From c03facdb0308417091d9fe8b8e2465a164fc9ab8 Mon Sep 17 00:00:00 2001 From: icarpis Date: Thu, 2 Nov 2017 13:21:45 +0200 Subject: [PATCH 01/43] Raspberry Pi 3 MD file --- config/99-realsense-libusb.rules | 1 + doc/RaspberryPi3.md | 28 ++++++++++++++++++++++++++ doc/readme.md | 1 + src/ds5/ds5-active.cpp | 34 ++++++++++++++++++-------------- src/ds5/ds5-factory.cpp | 2 ++ src/ds5/ds5-private.cpp | 5 +++-- src/ds5/ds5-private.h | 7 +++++-- src/ds5/ds5-rolling-shutter.cpp | 3 ++- 8 files changed, 61 insertions(+), 20 deletions(-) create mode 100644 doc/RaspberryPi3.md diff --git a/config/99-realsense-libusb.rules b/config/99-realsense-libusb.rules index 1f5411c9f3..4e78d7df1d 100644 --- a/config/99-realsense-libusb.rules +++ b/config/99-realsense-libusb.rules @@ -11,6 +11,7 @@ SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0ad2", MODE:="066 SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0ad3", MODE:="0666", GROUP:="plugdev" SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0ad4", MODE:="0666", GROUP:="plugdev" SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0ad5", MODE:="0666", GROUP:="plugdev" +SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0ad6", MODE:="0666", GROUP:="plugdev" SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0af6", MODE:="0666", GROUP:="plugdev" SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0afe", MODE:="0666", GROUP:="plugdev" SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0aff", MODE:="0666", GROUP:="plugdev" diff --git a/doc/RaspberryPi3.md b/doc/RaspberryPi3.md new file mode 100644 index 0000000000..37c491cc15 --- /dev/null +++ b/doc/RaspberryPi3.md @@ -0,0 +1,28 @@ +# Intel® RealSense™ D400 cameras with Raspberry Pi + +This document describes how to use the Intel RealSense D400 cameras on Raspberry PI 3 Model B (which offers only USB2 interface). + +> **Note:** Both Raspberry Pi platform and USB2 support in general are experimental features and are **not** officially supported by Intel RealSense group at this point. Camera capabilities are severely reduced when connected to USB2 port due to lack of bandwidth. + +

+ +## Installation Instructions +1. Install [Ubuntu MATE](https://ubuntu-mate.org/) on your [Raspberry PI 3](https://www.raspberrypi.org/products/raspberry-pi-3-model-b/). + +2. Clone and compile the latest [Intel® RealSense™ SDK 2.0](https://github.com/IntelRealSense/librealsense/releases/latest) by following the instructions under [Linux Installation](https://github.com/IntelRealSense/librealsense/blob/development/doc/installation.md). +In section [Video4Linux backend preparation](https://github.com/IntelRealSense/librealsense/blob/development/doc/installation.md#video4linux-backend-preparation), perform only the first phase "Install udev rules located in librealsense source directory". + +> **Note**: In some cases the RAM capacity is not sufficient to compile the SDK, so if the compilation process crashes or exits with an error-code try to create a [swap file](https://www.howtoforge.com/ubuntu-swap-file) and then recompile the SDK. + +3. [Activate](https://ubuntu-mate.community/t/tutorial-activate-opengl-driver-for-ubuntu-mate-16-04/7094) the OpenGL driver. +4. Plug the D400 camera and play with RealSense SDK [examples](https://github.com/IntelRealSense/librealsense/tree/master/examples)/[tools](https://github.com/IntelRealSense/librealsense/tree/master/tools). + +## Expected Output +1. Streaming Depth data with 480x270 resolution and 30 FPS using the [Intel RealSense Viewer](https://github.com/IntelRealSense/librealsense/tree/master/tools/realsense-viewer). +

+ +2. Streaming Depth and IR with 424x240 resolution and 30 FPS: +

+ +3. Point-cloud of depth and IR: +

diff --git a/doc/readme.md b/doc/readme.md index 1ef83de43a..d9c4944f29 100644 --- a/doc/readme.md +++ b/doc/readme.md @@ -24,4 +24,5 @@ * [D400 at software.intel.com](https://software.intel.com/en-us/realsense/d400) - Camera specifications * [D400 and External Devices](rs400/external_devices.md) - Notes on integrating RS400 with external devices * [D400 Advanced Mode](rs400/rs400_advanced_mode.md) - Overview of the Advanced Mode APIs + * [D400 cameras with Raspberry Pi](./RaspberryPi3.md) - Example of low-end system without USB3 interface * [Record and Playback](../src/media/readme.md) - SDK Record and Playback functionality using ROS-bag file format diff --git a/src/ds5/ds5-active.cpp b/src/ds5/ds5-active.cpp index 76a99f01ec..eae0dda717 100644 --- a/src/ds5/ds5-active.cpp +++ b/src/ds5/ds5-active.cpp @@ -25,22 +25,26 @@ namespace librealsense { using namespace ds; - auto& depth_ep = get_depth_sensor(); - auto emitter_enabled = std::make_shared(depth_ep); - depth_ep.register_option(RS2_OPTION_EMITTER_ENABLED, emitter_enabled); + auto pid = group.uvc_devices.front().pid; + if (pid != RS_USB2_PID) + { + auto& depth_ep = get_depth_sensor(); + auto emitter_enabled = std::make_shared(depth_ep); + depth_ep.register_option(RS2_OPTION_EMITTER_ENABLED, emitter_enabled); - auto laser_power = std::make_shared>(depth_ep, - depth_xu, - DS5_LASER_POWER, - "Manual laser power in mw. applicable only when laser power mode is set to Manual"); - depth_ep.register_option(RS2_OPTION_LASER_POWER, - std::make_shared( - laser_power, - emitter_enabled, - std::vector{0.f, 2.f}, 1.f)); + auto laser_power = std::make_shared>(depth_ep, + depth_xu, + DS5_LASER_POWER, + "Manual laser power in mw. applicable only when laser power mode is set to Manual"); + depth_ep.register_option(RS2_OPTION_LASER_POWER, + std::make_shared( + laser_power, + emitter_enabled, + std::vector{0.f, 2.f}, 1.f)); - depth_ep.register_option(RS2_OPTION_PROJECTOR_TEMPERATURE, - std::make_shared(depth_ep, - RS2_OPTION_PROJECTOR_TEMPERATURE)); + depth_ep.register_option(RS2_OPTION_PROJECTOR_TEMPERATURE, + std::make_shared(depth_ep, + RS2_OPTION_PROJECTOR_TEMPERATURE)); + } } } diff --git a/src/ds5/ds5-factory.cpp b/src/ds5/ds5-factory.cpp index 5b0b2e798a..574e84cf4f 100644 --- a/src/ds5/ds5-factory.cpp +++ b/src/ds5/ds5-factory.cpp @@ -207,6 +207,8 @@ namespace librealsense return std::make_shared(ctx, group, register_device_notifications); case RS435_RGB_PID: return std::make_shared(ctx, group, register_device_notifications); + case RS_USB2_PID: + return std::make_shared(ctx, group, register_device_notifications); default: throw std::runtime_error("Unsupported RS400 model!"); } diff --git a/src/ds5/ds5-private.cpp b/src/ds5/ds5-private.cpp index d4958c3e08..47a1b624ad 100644 --- a/src/ds5/ds5-private.cpp +++ b/src/ds5/ds5-private.cpp @@ -166,16 +166,17 @@ namespace librealsense result = *it; switch (info.pid) { + case RS_USB2_PID: case RS400_PID: case RS410_PID: case RS460_PID: case RS430_PID: case RS420_PID: - found = result.mi == 3; + found = (result.mi == 3); break; case RS430_MM_PID: case RS420_MM_PID: - found = result.mi == 6; + found = (result.mi == 6); break; case RS415_PID: case RS435_RGB_PID: diff --git a/src/ds5/ds5-private.h b/src/ds5/ds5-private.h index 333a70da63..66bc46d769 100644 --- a/src/ds5/ds5-private.h +++ b/src/ds5/ds5-private.h @@ -27,7 +27,8 @@ namespace librealsense const uint16_t RS430_MM_PID = 0x0ad5; // AWGT const uint16_t RS430_MM_RGB_PID = 0x0b01; // AWGCT const uint16_t RS435_RGB_PID = 0x0b07; // AWGC - const uint16_t RS460_PID = 0x0b03; // DS5U + const uint16_t RS460_PID = 0x0b03; // DS5U + const uint16_t RS_USB2_PID = 0x0ad6; // USB2 // DS5 depth XU identifiers const uint8_t DS5_HWMONITOR = 1; @@ -52,7 +53,8 @@ namespace librealsense ds::RS430_MM_PID, ds::RS430_MM_RGB_PID, ds::RS435_RGB_PID, - ds::RS460_PID + ds::RS460_PID, + ds::RS_USB2_PID }; static const std::map rs400_sku_names = { @@ -68,6 +70,7 @@ namespace librealsense { RS430_MM_PID, "Intel RealSense 430 with Tracking Module and RGB Module"}, { RS435_RGB_PID,"Intel RealSense 435"}, { RS460_PID, "Intel RealSense 460" }, + { RS_USB2_PID, "Intel RealSense USB2" } }; // DS5 fisheye XU identifiers diff --git a/src/ds5/ds5-rolling-shutter.cpp b/src/ds5/ds5-rolling-shutter.cpp index 20fdbde008..8ffe673ac2 100644 --- a/src/ds5/ds5-rolling-shutter.cpp +++ b/src/ds5/ds5-rolling-shutter.cpp @@ -25,7 +25,8 @@ namespace librealsense { using namespace ds; - if (_fw_version >= firmware_version("5.5.8.0")) + auto pid = group.uvc_devices.front().pid; + if ((_fw_version >= firmware_version("5.5.8.0")) && (pid != RS_USB2_PID)) { get_depth_sensor().register_option(RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE, std::make_shared>(get_depth_sensor(), From 4b0bc061a6f33a467c99436899d5b1f8072d21d4 Mon Sep 17 00:00:00 2001 From: Ziv Shahaf Date: Thu, 2 Nov 2017 21:54:59 +0200 Subject: [PATCH 02/43] Added python binding for rs2::align --- wrappers/python/python.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/wrappers/python/python.cpp b/wrappers/python/python.cpp index 4fe60526f0..d6de0d4e76 100644 --- a/wrappers/python/python.cpp +++ b/wrappers/python/python.cpp @@ -483,6 +483,10 @@ PYBIND11_PLUGIN(NAME) { .def("colorize", &rs2::colorizer::colorize, "depth"_a) /*.def("__call__", &rs2::colorizer::operator())*/; + py::class_ align(m, "align"); + align.def(py::init(), "align_to"_a) + .def("proccess", &rs2::align::proccess, "depth"_a); + /* rs2_record_playback.hpp */ py::class_ playback(m, "playback"); playback.def(py::init(), "device"_a) From 20bd784afad1f620d1128c79f8796aefac77190b Mon Sep 17 00:00:00 2001 From: Yunfei Hao Date: Fri, 3 Nov 2017 10:49:18 +0800 Subject: [PATCH 03/43] Update test cases due to latest APIs changing Impacted tests: new 0, update 20, delete 0 Unit test platform: Ubuntu 16.04 Unit test result summary: pass 20, fail 0, block 0 --- wrappers/nodejs/test/test-colorizer.js | 16 +++---- wrappers/nodejs/test/test-context.js | 16 +++---- wrappers/nodejs/test/test-depthsensor.js | 4 +- wrappers/nodejs/test/test-device.js | 16 +++---- wrappers/nodejs/test/test-frame.js | 3 +- wrappers/nodejs/test/test-pipeline.js | 5 +-- wrappers/nodejs/test/test-pointcloud.js | 8 ++-- wrappers/nodejs/test/test-points.js | 12 ++--- wrappers/nodejs/test/test-sensor.js | 56 +++++++++++++++++------- 9 files changed, 77 insertions(+), 59 deletions(-) diff --git a/wrappers/nodejs/test/test-colorizer.js b/wrappers/nodejs/test/test-colorizer.js index 867de5563b..6b05750f37 100644 --- a/wrappers/nodejs/test/test-colorizer.js +++ b/wrappers/nodejs/test/test-colorizer.js @@ -54,13 +54,13 @@ describe('Colorizer test', function() { let n = 0; while (!endTest) { const frameSet = pipeline.waitForFrames(); - const depthf = frameSet.depthFrame; - const colorf = frameSet.colorFrame; - frameSet.destroy(); n++; console.log(`retring left ...${10 - n} times`); - if (colorf !== undefined && - depthf !== undefined) { + if (frameSet !== undefined && + frameSet.colorFrame !== undefined && + frameSet.depthFrame !== undefined) { + const depthf = frameSet.depthFrame; + const colorf = frameSet.colorFrame; assert(depthf instanceof rs2.DepthFrame); assert(colorf instanceof rs2.VideoFrame); let videoFrame; @@ -69,19 +69,19 @@ describe('Colorizer test', function() { videoFrame = colorizer.colorize(depthf); }); assert(videoFrame instanceof rs2.VideoFrame); - assert.doesNotThrow(() => { // jshint ignore:line + assert.throws(() => { // jshint ignore:line videoFrameNull = colorizer.colorize(); }); assert(videoFrameNull === undefined); colorizer.destroy(); + if (colorf) colorf.destroy(); + if (depthf) depthf.destroy(); endTest = true; if (videoFrame) videoFrame.destroy(); } if (n >= 10) { assert(false, 'could not get colorFrame or depthFrame, try to reset camera'); } - if (colorf) colorf.destroy(); - if (depthf) depthf.destroy(); } pipeline.destroy(); }); diff --git a/wrappers/nodejs/test/test-context.js b/wrappers/nodejs/test/test-context.js index 9f48bd4c64..5d196b17dc 100644 --- a/wrappers/nodejs/test/test-context.js +++ b/wrappers/nodejs/test/test-context.js @@ -29,20 +29,18 @@ describe('Context test', function() { assert(context._events instanceof EventEmitter); }); - it('testing constructor - new Pointcloud, should get Pointcloud', () => { - let pointcloud; + it('testing constructor - new Context, should get Context', () => { + let context; assert.doesNotThrow(() => { - pointcloud = new librealsense2.Pointcloud(); + context = new librealsense2.Context(); }); - assert(pointcloud instanceof librealsense2.Pointcloud); + assert(context instanceof librealsense2.Context); }); - it('testing constructor - new Pointcloud, invalid 1 option', () => { - let pointcloud; - assert.doesNotThrow(() => { - pointcloud = new librealsense2.Pointcloud(1); + it('testing constructor - new Context, invalid 1 option', () => { + assert.throws(() => { + new librealsense2.Context(1); }); - assert(pointcloud instanceof librealsense2.Pointcloud); }); it('testing method - destroy, call 1 time', () => { diff --git a/wrappers/nodejs/test/test-depthsensor.js b/wrappers/nodejs/test/test-depthsensor.js index cc7bb19bad..d2f17923c5 100644 --- a/wrappers/nodejs/test/test-depthsensor.js +++ b/wrappers/nodejs/test/test-depthsensor.js @@ -13,7 +13,7 @@ let depthSensor; describe('Device test', function() { before(function() { ctx = new rs2.Context(); - const devices = ctx.queryDevices(); + const devices = ctx.queryDevices().devices; assert(devices.length > 0); // Device must be connected const dev = devices[0]; const pipeline = new rs2.Pipeline(); @@ -33,7 +33,7 @@ describe('Device test', function() { }); it('Testing constructor', () => { - assert.throws(() => { + assert.doesNotThrow(() => { new rs2.DepthFrame(); }); }); diff --git a/wrappers/nodejs/test/test-device.js b/wrappers/nodejs/test/test-device.js index 2fa3a0122f..6903ae5e3a 100644 --- a/wrappers/nodejs/test/test-device.js +++ b/wrappers/nodejs/test/test-device.js @@ -13,7 +13,7 @@ let dev; describe('Device test', function() { beforeEach(function() { ctx = new rs2.Context(); - const devices = ctx.queryDevices(); + const devices = ctx.queryDevices().devices; assert(devices.length > 0); // Device must be connected dev = devices[0]; }); @@ -45,24 +45,24 @@ describe('Device test', function() { it('Testing method getCameraInfo - return value', () => { let rtn = dev.getCameraInfo(); - // assert.equal(typeof rtn.deviceName, 'string'); - // assert.equal(typeof rtn.moduleName, 'string'); + // assert.equal(typeof rtn.deviceName, 'string'); // skip deviceName checking + // assert.equal(typeof rtn.moduleName, 'string'); // skip moduleName checking assert.equal(typeof rtn.serialNumber, 'string'); assert.equal(typeof rtn.firmwareVersion, 'string'); assert.equal(typeof rtn.location, 'string'); assert.equal(typeof rtn.debugOpCode, 'string'); assert.equal(typeof rtn.advancedMode, 'string'); assert.equal(typeof rtn.productId, 'string'); - // assert.equal(typeof rtn.cameraLocked, 'boolean'); + // assert.equal(typeof rtn.cameraLocked, 'boolean'); // skip cameraLocked checking }); it('Testing method getCameraInfo - with argument', () => { for (let i in rs2.camera_info) { if (rs2.camera_info[i] && - i.toUpperCase() !== 'CAMERA_INFO_LOCATION' && // BUG-7429 - i.toUpperCase() !== 'CAMERA_INFO_COUNT' && // skip counter - i.toUpperCase() !== 'CAMERA_INFO_CAMERA_LOCKED' && - i !== 'cameraInfoToString' // skip method + i.toUpperCase() !== 'CAMERA_INFO_LOCATION' && // location is not ready + i.toUpperCase() !== 'CAMERA_INFO_COUNT' && // skip counter + i.toUpperCase() !== 'CAMERA_INFO_CAMERA_LOCKED' && + i !== 'cameraInfoToString' // skip method ) { assert.doesNotThrow(() => { // jshint ignore:line dev.getCameraInfo(rs2.camera_info[i]); diff --git a/wrappers/nodejs/test/test-frame.js b/wrappers/nodejs/test/test-frame.js index 525834d8d8..befdfd9684 100644 --- a/wrappers/nodejs/test/test-frame.js +++ b/wrappers/nodejs/test/test-frame.js @@ -24,7 +24,7 @@ describe('Frame test', function() { }); it('Testing constructor', () => { - assert.throws(() => { + assert.doesNotThrow(() => { new rs2.Frame(); }); }); @@ -55,7 +55,6 @@ describe('Frame test', function() { assert.equal(typeof frame.timestamp, 'number'); }); - // DSO-7440 it.skip('Testing property streamType', () => { assert.equal(typeof frame.streamType, 'number'); }); diff --git a/wrappers/nodejs/test/test-pipeline.js b/wrappers/nodejs/test/test-pipeline.js index ed2e9f5dc7..ce59de8d7e 100644 --- a/wrappers/nodejs/test/test-pipeline.js +++ b/wrappers/nodejs/test/test-pipeline.js @@ -87,7 +87,7 @@ describe('Pipeline test', function() { pipeline.start(); pipeline.stop(); pipeline.destroy(); - }); + }); }); it('Testing method waitForFrames', () => { @@ -97,7 +97,6 @@ describe('Pipeline test', function() { pipeline = new rs2.Pipeline(); pipeline.start(); frameSet = pipeline.waitForFrames(); - frameSet.destroy(); }); let endTest = false; let n = 0; @@ -111,8 +110,6 @@ describe('Pipeline test', function() { assert(frameSet.colorFrame instanceof rs2.VideoFrame); endTest = true; } - // always destroy it - frameSet.destroy(); if (n >= 10) { assert(false, 'could not get colorFrame or depthFrame, try to reset camera'); } diff --git a/wrappers/nodejs/test/test-pointcloud.js b/wrappers/nodejs/test/test-pointcloud.js index 848be85ce3..2c5dc1e782 100644 --- a/wrappers/nodejs/test/test-pointcloud.js +++ b/wrappers/nodejs/test/test-pointcloud.js @@ -23,7 +23,7 @@ describe('Pointcloud test', function() { it('Testing method destroy', () => { let pointcloud; assert.doesNotThrow(() => { - pointcloud= new rs2.Pointcloud(); + pointcloud= new rs2.PointCloud(); pointcloud.destroy(); }); setTimeout(() => { @@ -37,7 +37,7 @@ describe('Pointcloud test', function() { let pointcloud; assert.doesNotThrow(() => { pipeline = new rs2.Pipeline(); - pointcloud = new rs2.Pointcloud(); + pointcloud = new rs2.PointCloud(); pipeline.start(); }); let endTest = false; @@ -56,7 +56,7 @@ describe('Pointcloud test', function() { points = pointcloud.calculate(frameSet.depthFrame); }); assert(points instanceof rs2.Points); - assert.doesNotThrow(() => { // jshint ignore:line + assert.throws(() => { // jshint ignore:line pointsNull = pointcloud.calculate(); }); assert(pointsNull === undefined); @@ -77,7 +77,7 @@ describe('Pointcloud test', function() { let pointcloud; assert.doesNotThrow(() => { pipeline = new rs2.Pipeline(); - pointcloud = new rs2.Pointcloud(); + pointcloud = new rs2.PointCloud(); pipeline.start(); }); let endTest = false; diff --git a/wrappers/nodejs/test/test-points.js b/wrappers/nodejs/test/test-points.js index 2f6bd6ecf1..30a26c4bbd 100644 --- a/wrappers/nodejs/test/test-points.js +++ b/wrappers/nodejs/test/test-points.js @@ -20,13 +20,13 @@ describe('Points test', function() { rs2.cleanup(); }); - it('Testing method getTextureCoordinates', () => { + it('Testing member textureCoordinates', () => { let pipeline; let frameSet; let pointcloud; assert.doesNotThrow(() => { pipeline = new rs2.Pipeline(); - pointcloud = new rs2.Pointcloud(); + pointcloud = new rs2.PointCloud(); pipeline.start(); }); let endTest = false; @@ -41,7 +41,7 @@ describe('Points test', function() { let arr; assert.doesNotThrow(() => { // jshint ignore:line points = pointcloud.calculate(frameSet.depthFrame); - arr = points.getTextureCoordinates(); + arr = points.textureCoordinates; }); assert.equal(typeof arr[0], 'number'); assert.equal(Object.prototype.toString.call(arr), '[object Int32Array]'); @@ -56,13 +56,13 @@ describe('Points test', function() { pipeline.destroy(); }); - it('Testing method getVertices', () => { + it('Testing member vertices', () => { let pipeline; let frameSet; let pointcloud; assert.doesNotThrow(() => { pipeline = new rs2.Pipeline(); - pointcloud = new rs2.Pointcloud(); + pointcloud = new rs2.PointCloud(); pipeline.start(); }); let endTest = false; @@ -77,7 +77,7 @@ describe('Points test', function() { let arr; assert.doesNotThrow(() => { // jshint ignore:line points = pointcloud.calculate(frameSet.depthFrame); - arr = points.getVertices(); + arr = points.vertices; }); assert.equal(typeof arr[0], 'number'); assert.equal(Object.prototype.toString.call(arr), '[object Float32Array]'); diff --git a/wrappers/nodejs/test/test-sensor.js b/wrappers/nodejs/test/test-sensor.js index 342a930b2d..d6cc01cd82 100644 --- a/wrappers/nodejs/test/test-sensor.js +++ b/wrappers/nodejs/test/test-sensor.js @@ -49,10 +49,18 @@ describe('Sensor test', function() { sensors.forEach((sensor) => { Object.keys(rs2.option).forEach((o) => { if (o === 'OPTION_COUNT' || o === 'optionToString') return; - assert.equal(typeof sensor.isOptionReadOnly(rs2.option[o]), 'boolean'); + if (sensor.supportsOption(rs2.option[o])) { + assert.equal(typeof sensor.isOptionReadOnly(rs2.option[o]), 'boolean'); + } else { + assert.equal(typeof sensor.isOptionReadOnly(rs2.option[o]), 'undefined'); + } }); optionsTestArray.forEach((o) => { - assert.equal(typeof sensor.isOptionReadOnly(o), 'boolean'); + if (sensor.supportsOption(o)) { + assert.equal(typeof sensor.isOptionReadOnly(o), 'boolean'); + } else { + assert.equal(typeof sensor.isOptionReadOnly(o), 'undefined'); + } }); }); }); @@ -103,10 +111,18 @@ describe('Sensor test', function() { sensors.forEach((sensor) => { Object.keys(rs2.option).forEach((o) => { if (o === 'OPTION_COUNT' || o === 'optionToString') return; - assert.equal(typeof sensor.getOption(rs2.option[o]), 'number'); + if (sensor.supportsOption(rs2.option[o])) { + assert.equal(typeof sensor.getOption(rs2.option[o]), 'number'); + } else { + assert.equal(typeof sensor.getOption(rs2.option[o]), 'undefined'); + } }); optionsTestArray.forEach((o) => { - assert.equal(typeof sensor.getOption(o), 'number'); + if (sensor.supportsOption(o)) { + assert.equal(typeof sensor.getOption(o), 'number'); + } else { + assert.equal(typeof sensor.getOption(o), 'undefined'); + } }); }); }); @@ -134,20 +150,28 @@ describe('Sensor test', function() { sensors.forEach((sensor) => { Object.keys(rs2.option).forEach((o) => { if (o === 'OPTION_COUNT' || o === 'optionToString') return; - const r = sensor.getOptionRange(rs2.option[o]); - assert.equal(typeof r, 'object'); - assert.equal(typeof r.minValue, 'number'); - assert.equal(typeof r.maxValue, 'number'); - assert.equal(typeof r.defaultValue, 'number'); - assert.equal(typeof r.step, 'number'); + if (sensor.supportsOption(rs2.option[o])) { + const r = sensor.getOptionRange(rs2.option[o]); + assert.equal(typeof r, 'object'); + assert.equal(typeof r.minValue, 'number'); + assert.equal(typeof r.maxValue, 'number'); + assert.equal(typeof r.defaultValue, 'number'); + assert.equal(typeof r.step, 'number'); + } else { + assert.equal(typeof sensor.getOptionRange(rs2.option[o]), 'undefined'); + } }); optionsTestArray.forEach((o) => { - const r = sensor.getOptionRange(o); - assert.equal(typeof r, 'object'); - assert.equal(typeof r.minValue, 'number'); - assert.equal(typeof r.maxValue, 'number'); - assert.equal(typeof r.defaultValue, 'number'); - assert.equal(typeof r.step, 'number'); + if (sensor.supportsOption(o)) { + const r = sensor.getOptionRange(o); + assert.equal(typeof r, 'object'); + assert.equal(typeof r.minValue, 'number'); + assert.equal(typeof r.maxValue, 'number'); + assert.equal(typeof r.defaultValue, 'number'); + assert.equal(typeof r.step, 'number'); + } else { + assert.equal(typeof sensor.getOptionRange(o), 'undefined'); + } }); }); }); From 94a2c9876564bf48bb8eb39011c718681dc8f491 Mon Sep 17 00:00:00 2001 From: Yunfei Hao Date: Wed, 1 Nov 2017 14:27:40 +0800 Subject: [PATCH 04/43] Add nodejs test cases for frameset class Impacted tests: new 12, update 0, delete 0 Unit test platform: Ubuntu 16.04 Unit test result summary: pass 12, fail 0, block 0 --- wrappers/nodejs/test/test-frameset.js | 113 ++++++++++++++++++++++++++ 1 file changed, 113 insertions(+) create mode 100644 wrappers/nodejs/test/test-frameset.js diff --git a/wrappers/nodejs/test/test-frameset.js b/wrappers/nodejs/test/test-frameset.js new file mode 100644 index 0000000000..475f56af79 --- /dev/null +++ b/wrappers/nodejs/test/test-frameset.js @@ -0,0 +1,113 @@ +// Copyright (c) 2017 Intel Corporation. All rights reserved. +// Use of this source code is governed by an Apache 2.0 license +// that can be found in the LICENSE file. + +'use strict'; + +/* global describe, it, before, after */ +const assert = require('assert'); +const rs2 = require('../index.js'); + +let frameset; +describe('FrameSet test', function() { + before(function() { + const pipeline = new rs2.Pipeline(); + pipeline.start(); + while (frameset === undefined) { + const f = pipeline.waitForFrames(); + if (f.size > 1) { + frameset = f; + } + } + }); + + after(function() { + rs2.cleanup(); + }); + + it('Testing constructor', () => { + assert.doesNotThrow(() => { + new rs2.FrameSet(); + }); + }); + + it('Testing member size', () => { + assert.equal(typeof frameset.size, 'number'); + }); + + it('Testing member depthFrame', () => { + assert(frameset.depthFrame instanceof rs2.DepthFrame); + }); + + it('Testing member colorFrame', () => { + assert(frameset.colorFrame instanceof rs2.VideoFrame); + }); + + it('Testing method at - 0 argument', () => { + assert.doesNotThrow(() => { + frameset.at(); + }); + }); + + it('Testing method at - invalid argument', () => { + const len = frameset.size; + let f; + assert.doesNotThrow(() => { + f = frameset.at(len + 1); + }); + assert.equal(f, undefined); + }); + + it('Testing method at - valid argument', () => { + const len = frameset.size; + for (let i = 0; i < len; i++) { + let f; + assert.doesNotThrow(() => { // jshint ignore:line + f = frameset.at(i); + }); + assert(f instanceof rs2.DepthFrame || f instanceof rs2.VideoFrame); + } + }); + + it('Testing method getFrame - 0 argument', () => { + assert.throws(() => { + frameset.getFrame(); + }); + }); + + it('Testing method getFrame - invalid argument', () => { + assert.throws(() => { + frameset.getFrame('dummy'); + }); + }); + + it('Testing method getFrame - valid argument', () => { + for (let i in rs2.stream) { + if (rs2.stream[i] && + i.toUpperCase() !== 'STREAM_COUNT' && // skip counter + i !== 'streamToString' // skip method + ) { + assert.doesNotThrow(() => { // jshint ignore:line + frameset.getFrame(rs2.stream[i]); + }); + assert.doesNotThrow(() => { // jshint ignore:line + frameset.getFrame(rs2.stream[i]); + }); + } + } + }); + + it('Testing method getFrame - stream_depth', () => { + let d = frameset.getFrame(rs2.stream['stream_depth']); // jshint ignore:line + assert(d instanceof rs2.DepthFrame); + let D = frameset.getFrame(rs2.stream['STREAM_DEPTH']); // jshint ignore:line + assert(D instanceof rs2.DepthFrame); + }); + + it('Testing method getFrame - stream_color', () => { + let d = frameset.getFrame(rs2.stream['stream_color']); // jshint ignore:line + assert(d instanceof rs2.VideoFrame); + let D = frameset.getFrame(rs2.stream['STREAM_COLOR']); // jshint ignore:line + assert(D instanceof rs2.VideoFrame); + }); +}); From 4832a1e8e7832eee61ece5cadaf343bb6198362a Mon Sep 17 00:00:00 2001 From: Ziv Shahaf Date: Fri, 3 Nov 2017 15:15:35 +0200 Subject: [PATCH 05/43] Added align_depth2color example. Create exmaples folder foy python examples with readme --- wrappers/python/examples/align-depth2color.py | 77 +++++++++++++++++++ .../{ => examples}/opencv_viewer_example.py | 7 ++ .../pybackend_example_1_general.py | 2 +- .../python-rs400-advanced-mode-example.py | 0 .../{ => examples}/python-tutorial-1-depth.py | 2 +- wrappers/python/examples/readme.md | 12 +++ wrappers/python/readme.md | 20 +++-- 7 files changed, 110 insertions(+), 10 deletions(-) create mode 100644 wrappers/python/examples/align-depth2color.py rename wrappers/python/{ => examples}/opencv_viewer_example.py (82%) rename wrappers/python/{ => examples}/pybackend_example_1_general.py (97%) rename wrappers/python/{ => examples}/python-rs400-advanced-mode-example.py (100%) rename wrappers/python/{ => examples}/python-tutorial-1-depth.py (96%) create mode 100644 wrappers/python/examples/readme.md diff --git a/wrappers/python/examples/align-depth2color.py b/wrappers/python/examples/align-depth2color.py new file mode 100644 index 0000000000..420276e9bf --- /dev/null +++ b/wrappers/python/examples/align-depth2color.py @@ -0,0 +1,77 @@ +## License: Apache 2.0. See LICENSE file in root directory. +## Copyright(c) 2017 Intel Corporation. All Rights Reserved. + +##################################################### +## Align Depth to Color ## +##################################################### + +# First import the library +import pyrealsense2 as rs +# Import Numpy for easy array manipulation +import numpy as np +# Import OpenCV for easy image rendering +import cv2 + +# Create a pipeline +pipeline = rs.pipeline() + +#Create a config and configure the pipeline to stream +# different resolutions of color and depth streams +config = rs.config() +config.enable_stream(rs.stream.depth, 640, 360, rs.format.z16, 30) +config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30) + +# Start streaming +profile = pipeline.start(config) + +# Getting the depth sensor's depth scale (see rs-align example for explanation) +depth_sensor = profile.get_device().first_depth_sensor() +depth_scale = depth_sensor.get_depth_scale() +print "Depth Scale is: " , depth_scale + +# We will be removing the background of objects more than +# clipping_distance_in_meters meters away +clipping_distance_in_meters = 1 #1 meter +clipping_distance = clipping_distance_in_meters / depth_scale + +# Create an align object +# rs.align allows us to perform alignment of depth frames to others frames +# The "align_to" is the stream type to which we plan to align depth frames. +align_to = rs.stream.color +align = rs.align(align_to) + +# Streaming loop +try: + while True: + # Get frameset of color and depth + frames = pipeline.wait_for_frames() + # frames.get_depth_frame() is a 640x360 depth image + + # Align the depth frame to color frame + aligned_frames = align.proccess(frames) + + # Get aligned frames + aligned_depth_frame = aligned_frames.get_depth_frame() # aligned_depth_frame is a 640x480 depth image + color_frame = aligned_frames.get_color_frame() + + # Validate that both frames are valid + if not aligned_depth_frame or not color_frame: + continue + + depth_image = np.asanyarray(aligned_depth_frame.get_data()) + color_image = np.asanyarray(color_frame.get_data()) + + # Remove background - Set pixels further than clipping_distance to grey + grey_color = 153 + depth_image_3d = np.dstack((depth_image,depth_image,depth_image)) #depth image is 1 channel, color is 3 channels + bg_removed = np.where((depth_image_3d > clipping_distance) | (depth_image_3d <= 0), grey_color, color_image)#& (depth_image_3d < 0.1) + + # Render images + depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, None, 0.5, 0), cv2.COLORMAP_JET) + images = np.hstack((bg_removed, depth_colormap)) + cv2.namedWindow('Align Example', cv2.WINDOW_AUTOSIZE) + cv2.imshow('Align Example', images) + cv2.waitKey(1) +finally: + pipeline.stop() + diff --git a/wrappers/python/opencv_viewer_example.py b/wrappers/python/examples/opencv_viewer_example.py similarity index 82% rename from wrappers/python/opencv_viewer_example.py rename to wrappers/python/examples/opencv_viewer_example.py index 3c069a72fe..642188e5ab 100644 --- a/wrappers/python/opencv_viewer_example.py +++ b/wrappers/python/examples/opencv_viewer_example.py @@ -1,3 +1,10 @@ +## License: Apache 2.0. See LICENSE file in root directory. +## Copyright(c) 2015-2017 Intel Corporation. All Rights Reserved. + +############################################### +## Open CV and Numpy integration ## +############################################### + import pyrealsense2 as rs import numpy as np import cv2 diff --git a/wrappers/python/pybackend_example_1_general.py b/wrappers/python/examples/pybackend_example_1_general.py similarity index 97% rename from wrappers/python/pybackend_example_1_general.py rename to wrappers/python/examples/pybackend_example_1_general.py index 9fcde5d8e3..910b6f1b40 100644 --- a/wrappers/python/pybackend_example_1_general.py +++ b/wrappers/python/examples/pybackend_example_1_general.py @@ -1,5 +1,5 @@ ## License: Apache 2.0. See LICENSE file in root directory. -## Copyright(c) 2015 Intel Corporation. All Rights Reserved. +## Copyright(c) 2015-2017 Intel Corporation. All Rights Reserved. ############################################### ## pybackend example #1 - A general overview ## diff --git a/wrappers/python/python-rs400-advanced-mode-example.py b/wrappers/python/examples/python-rs400-advanced-mode-example.py similarity index 100% rename from wrappers/python/python-rs400-advanced-mode-example.py rename to wrappers/python/examples/python-rs400-advanced-mode-example.py diff --git a/wrappers/python/python-tutorial-1-depth.py b/wrappers/python/examples/python-tutorial-1-depth.py similarity index 96% rename from wrappers/python/python-tutorial-1-depth.py rename to wrappers/python/examples/python-tutorial-1-depth.py index 3cebd581f4..5dc5328c8a 100644 --- a/wrappers/python/python-tutorial-1-depth.py +++ b/wrappers/python/examples/python-tutorial-1-depth.py @@ -1,5 +1,5 @@ ## License: Apache 2.0. See LICENSE file in root directory. -## Copyright(c) 2015 Intel Corporation. All Rights Reserved. +## Copyright(c) 2015-2017 Intel Corporation. All Rights Reserved. ##################################################### ## librealsense tutorial #1 - Accessing depth data ## diff --git a/wrappers/python/examples/readme.md b/wrappers/python/examples/readme.md new file mode 100644 index 0000000000..b688f3c780 --- /dev/null +++ b/wrappers/python/examples/readme.md @@ -0,0 +1,12 @@ + +# Sample Code for Intel® RealSense™ Python Wrapper + +These Examples demonstrate how to use the python wrapper of the SDK. + +## List of Exmples: + +1. [Tutorial 1](./python-tutorial-1-depth.py) - Demonstrates how to start streaming depth frames from the camera and display the image in the console as an ASCII art. +2. [NumPy and OpenCV](./opencv_viewer_example.py) - Example of rendering depth and color images using the help of OpenCV and Numpy +3. [Stream Alignment](./align-depth2color.py) - Demonstrate a way of performing background removal by aligning depth images to color images and performing simple calculation to strip the background. +4. [RS400 Advanced Mode](./python-rs400-advanced-mode-example.py) - Example of the advanced mode interface for controlling different options of the D400 cameras +5. [Realsense Backend](./pybackend_example_1_general.py) - Example of controlling devices using the backend interface diff --git a/wrappers/python/readme.md b/wrappers/python/readme.md index 3f179cd831..5cad69bd85 100644 --- a/wrappers/python/readme.md +++ b/wrappers/python/readme.md @@ -22,8 +22,17 @@ 1. Install Python 2 or 3 for windows * You can find the downloads on the official Python website [here](https://www.python.org/downloads/windows/) 2. When running cmake, select the BUILD_PYTHON_BINDINGS option +> `-DBUILD_PYTHON_BINDINGS=true` +3. If you have multiple python installations on your machine you can use: +> `-DPYTHON_EXECUTABLE= For a list of full code examples see the [examples](./examples) folder + +#### Streaming using _rs.pipeline_ ```python # First import the library import pyrealsense2 as rs @@ -47,7 +56,7 @@ try: dist = depth.get_distance(x, y) if 0 < dist and dist < 1: coverage[x/10] += 1 - + if y%20 is 19: line = "" for c in coverage: @@ -56,12 +65,7 @@ try: print(line) ``` -Additionally the following examples are available: - - [Tutorial 1](./python-tutorial-1-depth.py) - - [RS400 Advanced Mode](./python-rs400-advanced-mode-example.py) - - [Backend Example](./pybackend_example_1_general.py) - -### NumPy Integration +#### NumPy Integration Librealsense frames support the buffer protocol. A numpy array can be constructed using this protocol with no data marshalling overhead: ```python import numpy as np @@ -69,4 +73,4 @@ depth = frames.get_depth_frame() depth_data = depth.as_frame().get_data() np_image = np.asanyarray(depth_data) ``` -A simple viewer example with NumPy and OpenCV can be found [here](./opencv_viewer_example.py) \ No newline at end of file + From 0220bab547907a7e750f50b38aa5d7e7e87d956c Mon Sep 17 00:00:00 2001 From: icarpis Date: Sun, 5 Nov 2017 15:55:28 +0200 Subject: [PATCH 06/43] Fixed bug DSO-6345 - DMFT --- src/win/win-uvc.cpp | 72 ++++++++++++++++++++++++--------------------- src/win/win-uvc.h | 16 ++++++++++ 2 files changed, 54 insertions(+), 34 deletions(-) diff --git a/src/win/win-uvc.cpp b/src/win/win-uvc.cpp index f57fd76890..981a823522 100644 --- a/src/win/win-uvc.cpp +++ b/src/win/win-uvc.cpp @@ -692,46 +692,50 @@ namespace librealsense void wmf_uvc_device::foreach_uvc_device(enumeration_callback action) { - CComPtr pAttributes = nullptr; - CHECK_HR(MFCreateAttributes(&pAttributes, 1)); - CHECK_HR(pAttributes->SetGUID( - MF_DEVSOURCE_ATTRIBUTE_SOURCE_TYPE, - MF_DEVSOURCE_ATTRIBUTE_SOURCE_TYPE_VIDCAP_GUID)); - - IMFActivate ** ppDevices; - UINT32 numDevices; - CHECK_HR(MFEnumDeviceSources(pAttributes, &ppDevices, &numDevices)); - - for (UINT32 i = 0; i < numDevices; ++i) + for (auto attributes_params_set : attributes_params) { - CComPtr pDevice; - *&pDevice = ppDevices[i]; - - WCHAR * wchar_name = nullptr; UINT32 length; - CHECK_HR(pDevice->GetAllocatedString(MF_DEVSOURCE_ATTRIBUTE_SOURCE_TYPE_VIDCAP_SYMBOLIC_LINK, &wchar_name, &length)); - auto name = win_to_utf(wchar_name); - CoTaskMemFree(wchar_name); - - uint16_t vid, pid, mi; std::string unique_id; - if (!parse_usb_path(vid, pid, mi, unique_id, name)) continue; - - uvc_device_info info; - info.vid = vid; - info.pid = pid; - info.unique_id = unique_id; - info.mi = mi; - info.device_path = name; - try + CComPtr pAttributes = nullptr; + CHECK_HR(MFCreateAttributes(&pAttributes, 1)); + for (auto attribute_params : attributes_params_set) { - action(info, ppDevices[i]); + CHECK_HR(pAttributes->SetGUID(attribute_params.first, attribute_params.second)); } - catch (...) + + IMFActivate ** ppDevices; + UINT32 numDevices; + CHECK_HR(MFEnumDeviceSources(pAttributes, &ppDevices, &numDevices)); + + for (UINT32 i = 0; i < numDevices; ++i) { - // TODO + CComPtr pDevice; + *&pDevice = ppDevices[i]; + + WCHAR * wchar_name = nullptr; UINT32 length; + CHECK_HR(pDevice->GetAllocatedString(MF_DEVSOURCE_ATTRIBUTE_SOURCE_TYPE_VIDCAP_SYMBOLIC_LINK, &wchar_name, &length)); + auto name = win_to_utf(wchar_name); + CoTaskMemFree(wchar_name); + + uint16_t vid, pid, mi; std::string unique_id; + if (!parse_usb_path(vid, pid, mi, unique_id, name)) continue; + + uvc_device_info info; + info.vid = vid; + info.pid = pid; + info.unique_id = unique_id; + info.mi = mi; + info.device_path = name; + try + { + action(info, ppDevices[i]); + } + catch (...) + { + // TODO + } } - } - CoTaskMemFree(ppDevices); + CoTaskMemFree(ppDevices); + } } void wmf_uvc_device::set_power_state(power_state state) diff --git a/src/win/win-uvc.h b/src/win/win-uvc.h index 603531aba5..135f6dd20a 100644 --- a/src/win/win-uvc.h +++ b/src/win/win-uvc.h @@ -15,6 +15,22 @@ #include #include +#ifndef KSCATEGORY_SENSOR_CAMERA +DEFINE_GUIDSTRUCT("24E552D7-6523-47F7-A647-D3465BF1F5CA", KSCATEGORY_SENSOR_CAMERA); +#define KSCATEGORY_SENSOR_CAMERA DEFINE_GUIDNAMED(KSCATEGORY_SENSOR_CAMERA) +#endif // !KSCATEGORY_SENSOR_CAMERA + + +static const std::vector>> attributes_params = { + { + { MF_DEVSOURCE_ATTRIBUTE_SOURCE_TYPE, MF_DEVSOURCE_ATTRIBUTE_SOURCE_TYPE_VIDCAP_GUID }, + { MF_DEVSOURCE_ATTRIBUTE_SOURCE_TYPE_VIDCAP_CATEGORY, KSCATEGORY_SENSOR_CAMERA } + }, + { + { MF_DEVSOURCE_ATTRIBUTE_SOURCE_TYPE, MF_DEVSOURCE_ATTRIBUTE_SOURCE_TYPE_VIDCAP_GUID } + }, +}; + namespace librealsense { namespace platform From 2d859ca54beee4655093c0fc7dc5a0fc4e20798b Mon Sep 17 00:00:00 2001 From: Ziv Shahaf Date: Tue, 7 Nov 2017 11:18:56 +0200 Subject: [PATCH 07/43] Added default window size and null validation for primary monitor pointer to allow default window size when in remote desktop connection (and no screen is connected) --- common/ux-window.cpp | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/common/ux-window.cpp b/common/ux-window.cpp index ebc3c4aed4..ac580d5545 100644 --- a/common/ux-window.cpp +++ b/common/ux-window.cpp @@ -22,12 +22,19 @@ namespace rs2 rs2_error* e = nullptr; _title_str = to_string() << title << " v" << api_version_to_string(rs2_get_api_version(&e)); + _width = 1024; + _height = 768; + + // Dynamically adjust new window size (by detecting monitor resolution) auto primary = glfwGetPrimaryMonitor(); - const auto mode = glfwGetVideoMode(primary); + if (primary) + { + const auto mode = glfwGetVideoMode(primary); + _width = int(mode->width * 0.7f); + _height = int(mode->height * 0.7f); + } // Create GUI Windows - _width = int(mode->width * 0.7f); - _height = int(mode->height * 0.7f); _win = glfwCreateWindow(_width, _height, _title_str.c_str(), nullptr, nullptr); glfwMakeContextCurrent(_win); ImGui_ImplGlfw_Init(_win, true); From e292973ddd3f514718d03b4d30e762fe5397f9eb Mon Sep 17 00:00:00 2001 From: Ziv Shahaf Date: Tue, 7 Nov 2017 13:40:27 +0200 Subject: [PATCH 08/43] Fixed python binding for rs.pointcloud (missing ctor) --- wrappers/python/python.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/wrappers/python/python.cpp b/wrappers/python/python.cpp index d6de0d4e76..d44594bd46 100644 --- a/wrappers/python/python.cpp +++ b/wrappers/python/python.cpp @@ -463,8 +463,9 @@ PYBIND11_PLUGIN(NAME) { .def("__call__", &rs2::frame_queue::operator()); py::class_ pointcloud(m, "pointcloud"); - pointcloud.def("calculate", &rs2::pointcloud::calculate, "depth"_a) - .def("map_to", &rs2::pointcloud::map_to, "mapped"_a); + pointcloud.def(py::init<>()) + .def("calculate", &rs2::pointcloud::calculate, "depth"_a) + .def("map_to", &rs2::pointcloud::map_to, "mapped"_a); py::class_ syncer(m, "syncer"); syncer.def(py::init<>()) From 029cb98a0a19426cff7c6e7fb29d9ace50c21269 Mon Sep 17 00:00:00 2001 From: icarpis Date: Tue, 7 Nov 2017 17:56:34 +0200 Subject: [PATCH 09/43] Workaround for bug DSO-7649 --- src/ds5/advanced_mode/advanced_mode.cpp | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/src/ds5/advanced_mode/advanced_mode.cpp b/src/ds5/advanced_mode/advanced_mode.cpp index 0b390e9f2f..7ee18dcec2 100644 --- a/src/ds5/advanced_mode/advanced_mode.cpp +++ b/src/ds5/advanced_mode/advanced_mode.cpp @@ -414,7 +414,19 @@ namespace librealsense void ds5_advanced_mode_base::set_census_radius(const STCensusRadius& val) { - set(val, advanced_mode_traits::group); + ///////////////////////////////////////////////////////////////////////////////////////// + // Workaround for bug DSO-7649 Can't load visual presets with LibRS2.8.1 and FW 5.8.13 // + ///////////////////////////////////////////////////////////////////////////////////////// + static const int min = 1; + static const int max = 9; + auto temp_val(val); + if (temp_val.uDiameter < min || temp_val.uDiameter > max) + temp_val.uDiameter = max; + if (temp_val.vDiameter < min || temp_val.vDiameter > max) + temp_val.vDiameter = max; + ///////////////////////////////////////////////////////////////////////////////////////// + + set(temp_val, advanced_mode_traits::group); } void ds5_advanced_mode_base::set_laser_power(const laser_power_control& val) From 3b546e445afec554d91a9210f4a2ca1fd1a73f40 Mon Sep 17 00:00:00 2001 From: aangerma Date: Wed, 8 Nov 2017 10:17:03 +0200 Subject: [PATCH 10/43] Fixed issue in 3D viewer - will pass to syncer before calculating point-cloud. --- common/model-views.cpp | 188 +++++++++++++------- common/model-views.h | 53 ++++-- src/device.cpp | 4 +- src/ds5/ds5-color.cpp | 3 +- src/ds5/ds5-device.cpp | 7 +- src/ds5/ds5-motion.cpp | 3 +- src/image.cpp | 2 +- src/ivcam/sr300.cpp | 9 +- src/linux/backend-v4l2.cpp | 2 +- src/media/playback/playback_device.cpp | 3 +- src/sync.cpp | 85 ++++++--- src/sync.h | 50 +++++- tools/realsense-viewer/realsense-viewer.cpp | 20 ++- 13 files changed, 290 insertions(+), 139 deletions(-) diff --git a/common/model-views.cpp b/common/model-views.cpp index 155e969106..a409fe8d93 100644 --- a/common/model-views.cpp +++ b/common/model-views.cpp @@ -1113,15 +1113,22 @@ namespace rs2 _pause = false; } - void subdevice_model::play(const std::vector& profiles) + void subdevice_model::play(const std::vector& profiles, viewer_model& viewer) { s->open(profiles); try { s->start([&](frame f) { auto index = f.get_profile().unique_id(); - queues.at(index).enqueue(std::move(f)); + if (viewer.syncronize) + { + if (index == viewer.selected_depth_source_uid || index == viewer.selected_tex_source_uid) + viewer.s(f); + } + queues.at(index).enqueue(f); }); + } + catch (...) { s->close(); @@ -1493,7 +1500,6 @@ namespace rs2 // } //} - auto model = pc.get_points(); const auto top_bar_height = 32.f; const auto num_of_buttons = 4; @@ -1657,17 +1663,6 @@ namespace rs2 //} //ImGui::PopItemWidth(); - if (selected_depth_source_uid >= 0) { - auto depth = streams[selected_depth_source_uid].texture->get_last_frame(); - if (depth) pc.push_frame(depth); - } - - frame tex; - if (selected_tex_source_uid >= 0) - { - tex = streams[selected_tex_source_uid].texture->get_last_frame(); - if (tex) pc.update_texture(tex); - } ImGui::SetCursorPos({ stream_rect.w - 32 * num_of_buttons - 5, 0 }); @@ -1713,6 +1708,15 @@ namespace rs2 { if (auto ret = file_dialog_open(save_file, "Polygon File Format (PLY)\0*.ply\0", NULL, NULL)) { + auto model = pc.get_points(); + + frame tex; + if (selected_tex_source_uid >= 0) + { + tex = streams[selected_tex_source_uid].texture->get_last_frame(); + if (tex) pc.update_texture(tex); + } + std::string fname(ret); if (!ends_with(to_lower(fname), ".ply")) fname += ".ply"; export_to_ply(fname.c_str(), not_model, model, tex); @@ -1801,6 +1805,7 @@ namespace rs2 void viewer_model::gc_streams() { + std::lock_guard lock(streams_mutex); std::vector streams_to_remove; for (auto&& kvp : streams) { @@ -2363,7 +2368,7 @@ namespace rs2 if (profiles.empty()) continue; - sub->play(profiles); + sub->play(profiles, viewer); for (auto&& profile : profiles) { @@ -2598,32 +2603,93 @@ namespace rs2 { show_icon(font_18, "recording_icon", u8"\uf111", x, y, id, from_rgba(255, 46, 54, alpha_delta * 255)); } + + void async_pointclound_mapper::proccess(rs2::frame f, const rs2::frame_source& source) + { + + points p; + std::vector results; + + + if(viewer.syncronize) + { + if (auto composite = f.as()) + { + for(auto&& f: composite) + { + if(f.get_profile().unique_id() == viewer.selected_depth_source_uid) + { + p = pc.calculate(f); + results.push_back(std::move(p)); + + } + if(f.get_profile().unique_id() == viewer.selected_tex_source_uid) + { + update_texture(f); + results.push_back(std::move(f)); + } + } + } + + + auto res = source.allocate_composite_frame(results); + source.frame_ready(std::move(res)); + + + } + else + { + if(f.get_profile().unique_id() == viewer.selected_depth_source_uid) + { + + if(last_tex_frame) + results.push_back(last_tex_frame); + p = pc.calculate(f); + results.push_back(std::move(p)); + + auto res = source.allocate_composite_frame(results); + source.frame_ready(std::move(res)); + } + + } + } + void async_pointclound_mapper::render_loop() { while (keep_calculating_pointcloud) { try { - frame f; - if (depth_frames_to_render.poll_for_frame(&f)) + if(viewer.syncronize) { - if (f.get_frame_number() == last_frame_number && - f.get_timestamp() <= last_timestamp && - f.get_profile().unique_id() == last_stream_id) - continue; - - resulting_3d_models.enqueue(pc.calculate(f)); - - last_frame_number = f.get_frame_number(); - last_timestamp = f.get_timestamp(); - last_stream_id = f.get_profile().unique_id(); + frameset frames; + if (viewer.syncer_queue.poll_for_frame(&frames)) + { + processing_block.invoke(frames); + } + } + else + { + std::lock_guard lock(viewer.streams_mutex); + for (auto&& s : viewer.streams) + { + if(s.first == viewer.selected_tex_source_uid) + { + update_texture(s.second.texture->get_last_frame()); + last_tex_frame = s.second.texture->get_last_frame(); + } + if(s.first == viewer.selected_depth_source_uid) + { + processing_block.invoke(s.second.texture->get_last_frame()); + } + } } } catch (...) {} // There is no practical reason to re-calculate the 3D model // at higher frequency then 100 FPS - std::this_thread::sleep_for(std::chrono::milliseconds(10)); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); } } @@ -2925,40 +2991,26 @@ namespace rs2 glColor4f(1.f, 1.f, 1.f, 1.f); - if (syncronize) - { - auto tex = streams[selected_tex_source_uid].texture->get_last_frame(); - if (tex) s(tex); - } - if (auto points = pc.get_points()) + if(!paused) { - if (syncronize) - { - s(points); - rs2::frameset fs; - if (s.poll_for_frames(&fs)) - if (fs && fs.size() > 1) - { - for (auto&& f : fs) - { - if (f.is()) last_points = f; - else last_texture = f; - } - } - } - else + auto res = pc.get_points(); + + for (auto&& f : res) { - last_texture = streams[selected_tex_source_uid].texture->get_last_frame(); - last_points = points; + if (f.is()) + last_points = f; + else + last_texture = f; } - if (draw_frustrum) - { + } + if (draw_frustrum && last_points) + { glLineWidth(1.f); glBegin(GL_LINES); - auto intrin = points.get_profile().as().get_intrinsics(); + auto intrin = last_points.get_profile().as().get_intrinsics(); glColor4f(sensor_bg.x, sensor_bg.y, sensor_bg.z, 0.5f); @@ -2990,11 +3042,11 @@ namespace rs2 glColor4f(1.f, 1.f, 1.f, 1.f); } - if (last_points && last_texture) + if ( last_points && last_texture) { texture.upload(last_texture); - glPointSize((float)viewer_rect.w / points.get_profile().as().width()); + glPointSize((float)viewer_rect.w / last_points.get_profile().as().width()); if (selected_tex_source_uid >= 0) { @@ -3008,23 +3060,25 @@ namespace rs2 //glTexParameterfv(GL_TEXTURE_2D, GL_TEXTURE_BORDER_COLOR, tex_border_color); - glBegin(GL_POINTS); - auto vertices = last_points.get_vertices(); - auto tex_coords = last_points.get_texture_coordinates(); + glBegin(GL_POINTS); - for (int i = 0; i < last_points.size(); i++) - { - if (vertices[i].z) + auto vertices = last_points.get_vertices(); + auto tex_coords = last_points.get_texture_coordinates(); + + for (int i = 0; i < last_points.size(); i++) { - glVertex3fv(vertices[i]); - glTexCoord2fv(tex_coords[i]); + if (vertices[i].z) + { + glVertex3fv(vertices[i]); + glTexCoord2fv(tex_coords[i]); + } + } + glEnd(); - } - glEnd(); } - } + glDisable(GL_DEPTH_TEST); diff --git a/common/model-views.h b/common/model-views.h index f6b480bf27..0f062797a8 100644 --- a/common/model-views.h +++ b/common/model-views.h @@ -172,7 +172,8 @@ namespace rs2 std::map selected_format_id; }; - class subdevice_model; + class viewer_model; + class subdevice_model; class processing_block_model { @@ -209,7 +210,7 @@ namespace rs2 bool is_selected_combination_supported(); std::vector get_selected_profiles(); void stop(); - void play(const std::vector& profiles); + void play(const std::vector& profiles, viewer_model& viewer); void update(std::string& error_message, notifications_model& model); void draw_options(const std::vector& drawing_order, bool update_read_only_options, std::string& error_message, @@ -478,14 +479,21 @@ namespace rs2 std::string get_file_name(const std::string& path); + class viewer_model; class async_pointclound_mapper { public: - async_pointclound_mapper() - : keep_calculating_pointcloud(true), - resulting_3d_models(1), depth_frames_to_render(1), + async_pointclound_mapper(viewer_model& viewer) + : processing_block([&](rs2::frame f, const rs2::frame_source& source) + { + this->proccess(std::move(f),source); + }), + viewer(viewer), + keep_calculating_pointcloud(true), + resulting_3d_models(1), t([this]() {render_loop(); }) { + processing_block.start(resulting_3d_models); } ~async_pointclound_mapper() { stop(); } @@ -501,17 +509,13 @@ namespace rs2 } } - void push_frame(frame f) - { - depth_frames_to_render.enqueue(f); - } - - points get_points() + rs2::frameset get_points() { frame f; if (resulting_3d_models.poll_for_frame(&f)) { - model = f; + rs2::frameset frameset(f); + model = frameset; } return model; } @@ -521,16 +525,18 @@ namespace rs2 rs2::frame f{}; model = f; while (resulting_3d_models.poll_for_frame(&f)); - while (depth_frames_to_render.poll_for_frame(&f)); } private: - void render_loop(); + viewer_model& viewer; + void render_loop(); + void proccess(rs2::frame f, const rs2::frame_source& source); + rs2::frame last_tex_frame; + rs2::processing_block processing_block; pointcloud pc; - points model; + rs2::frameset model; std::atomic keep_calculating_pointcloud; - frame_queue depth_frames_to_render; frame_queue resulting_3d_models; std::thread t; @@ -552,12 +558,18 @@ namespace rs2 float get_output_height() const { return (is_output_collapsed ? default_log_h : 20); } viewer_model() + :pc(*this) { reset_camera(); rs2_error* e = nullptr; not_model.add_log(to_string() << "librealsense version: " << api_version_to_string(rs2_get_api_version(&e)) << "\n"); } + ~viewer_model() + { + streams.clear(); + pc.stop(); + } void upload_frame(frame&& f); std::map calc_layout(const rect& r); @@ -586,6 +598,7 @@ namespace rs2 void gc_streams(); + std::mutex streams_mutex; std::map streams; bool fullscreen = false; stream_model* selected_stream = nullptr; @@ -606,14 +619,18 @@ namespace rs2 bool draw_plane = false; bool draw_frustrum = true; - bool syncronize = false; + bool syncronize = true; int selected_depth_source_uid = -1; int selected_tex_source_uid = -1; float dim_level = 1.f; + rs2::syncer s; + rs2::frame_queue syncer_queue; private: + + friend class async_pointclound_mapper; std::map get_interpolated_layout(const std::map& l); void show_icon(ImFont* font_18, const char* label_str, const char* text, int x, int y, int id, const ImVec4& color, const std::string& tooltip = ""); @@ -633,7 +650,7 @@ namespace rs2 rs2::points last_points; rs2::frame last_texture; texture_buffer texture; - rs2::syncer s; + }; void export_to_ply(const std::string& file_name, notifications_model& ns, points points, video_frame texture); diff --git a/src/device.cpp b/src/device.cpp index 18c0df9b3f..22a2e02290 100644 --- a/src/device.cpp +++ b/src/device.cpp @@ -101,7 +101,9 @@ void device::hardware_reset() std::shared_ptr librealsense::device::create_matcher(const frame_holder& frame) const { - return std::make_shared( frame.frame->get_stream()->get_unique_id(), frame.frame->get_stream()->get_stream_type()); + + return std::make_shared( frame.frame->get_stream()->get_unique_id(), frame.frame->get_stream()->get_stream_type() , + generate_matcher_name(frame.frame->get_stream()->get_stream_type(), frame.frame->get_stream()->get_unique_id())); } std::pair librealsense::device::get_extrinsics(const stream_interface& stream) const diff --git a/src/ds5/ds5-color.cpp b/src/ds5/ds5-color.cpp index ddce42942f..394ca7bd83 100644 --- a/src/ds5/ds5-color.cpp +++ b/src/ds5/ds5-color.cpp @@ -171,6 +171,7 @@ namespace librealsense return device::create_matcher(frame); } - return std::make_shared( _color_stream->get_unique_id(), _color_stream->get_stream_type()); + return std::make_shared( _color_stream->get_unique_id(), _color_stream->get_stream_type(), + generate_matcher_name(_color_stream->get_stream_type(), _color_stream->get_unique_id())); } } diff --git a/src/ds5/ds5-device.cpp b/src/ds5/ds5-device.cpp index 3fb756d229..69bb56dcf0 100644 --- a/src/ds5/ds5-device.cpp +++ b/src/ds5/ds5-device.cpp @@ -192,6 +192,7 @@ namespace librealsense std::unique_ptr ds5_timestamp_reader_backup(new ds5_timestamp_reader(backend.create_time_service())); auto depth_ep = std::make_shared(this, std::make_shared(depth_devices), std::unique_ptr(new ds5_timestamp_reader_from_metadata(std::move(ds5_timestamp_reader_backup)))); + depth_ep->register_xu(depth_xu); // make sure the XU is initialized everytime we power the camera @@ -413,8 +414,10 @@ namespace librealsense std::vector> depth_matchers; for (auto& s : streams) - depth_matchers.push_back(std::make_shared( s->get_unique_id(), s->get_stream_type())); - + { + depth_matchers.push_back(std::make_shared( s->get_unique_id(), s->get_stream_type(), + generate_matcher_name(s->get_stream_type(), s->get_unique_id()))); + } return std::make_shared(depth_matchers); } } diff --git a/src/ds5/ds5-motion.cpp b/src/ds5/ds5-motion.cpp index 2a24c8027a..4e83116130 100644 --- a/src/ds5/ds5-motion.cpp +++ b/src/ds5/ds5-motion.cpp @@ -425,7 +425,8 @@ namespace librealsense std::vector> mm_matchers; for (auto& s : streams) - mm_matchers.push_back(std::make_shared( s->get_unique_id(), s->get_stream_type())); + mm_matchers.push_back(std::make_shared( s->get_unique_id(), s->get_stream_type(), + generate_matcher_name(s->get_stream_type(), s->get_unique_id()))); return std::make_shared(mm_matchers); } diff --git a/src/image.cpp b/src/image.cpp index fa33167290..b8c86fd707 100644 --- a/src/image.cpp +++ b/src/image.cpp @@ -782,7 +782,7 @@ namespace librealsense const native_pixel_format pf_y16 = { 'Y16 ', 1, 2,{ { true, &unpack_y16_from_y16_10, { { { RS2_STREAM_INFRARED, 1 }, RS2_FORMAT_Y16 } } } } }; const native_pixel_format pf_y8i = { 'Y8I ', 1, 2,{ { true, &unpack_y8_y8_from_y8i, { { { RS2_STREAM_INFRARED, 1 }, RS2_FORMAT_Y8 },{ { RS2_STREAM_INFRARED, 2 }, RS2_FORMAT_Y8 } } } } }; const native_pixel_format pf_y12i = { 'Y12I', 1, 3,{ { true, &unpack_y16_y16_from_y12i_10, { { { RS2_STREAM_INFRARED, 1 }, RS2_FORMAT_Y16 },{ { RS2_STREAM_INFRARED, 2 }, RS2_FORMAT_Y16 } } } } }; - const native_pixel_format pf_z16 = { 'Z16 ', 1, 2,{ { false, ©_pixels<2>, { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16 } } }, + const native_pixel_format pf_z16 = { 'Z16 ', 1, 2,{ { true, ©_pixels<2>, { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16 } } }, // The Disparity_Z is not applicable for D4XX. TODO - merge with INVZ when confirmed /*{ false, ©_pixels<2>, { { RS2_STREAM_DEPTH, RS2_FORMAT_DISPARITY16 } } }*/ } }; const native_pixel_format pf_invz = { 'Z16 ', 1, 2, { { false, ©_pixels<2>, { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16 } } } } }; diff --git a/src/ivcam/sr300.cpp b/src/ivcam/sr300.cpp index 2d36ddb6a6..16becac45c 100644 --- a/src/ivcam/sr300.cpp +++ b/src/ivcam/sr300.cpp @@ -313,12 +313,15 @@ namespace librealsense std::vector streams = { _depth_stream.get(), _ir_stream.get()}; for (auto& s : streams) - depth_matchers.push_back(std::make_shared( s->get_unique_id(), s->get_stream_type())); - + { + depth_matchers.push_back(std::make_shared( s->get_unique_id(), s->get_stream_type(), + generate_matcher_name(s->get_stream_type(),s->get_unique_id() ))); + } std::vector> matchers; matchers.push_back( std::make_shared(depth_matchers)); - auto color_matcher = std::make_shared( _color_stream->get_unique_id(), _color_stream->get_stream_type()); + auto color_matcher = std::make_shared( _color_stream->get_unique_id(), _color_stream->get_stream_type(), + generate_matcher_name(_color_stream->get_stream_type(), _color_stream->get_unique_id())); matchers.push_back(color_matcher); return std::make_shared(matchers); diff --git a/src/linux/backend-v4l2.cpp b/src/linux/backend-v4l2.cpp index 387f5c6416..2574124e28 100644 --- a/src/linux/backend-v4l2.cpp +++ b/src/linux/backend-v4l2.cpp @@ -520,7 +520,7 @@ namespace librealsense void v4l_uvc_device::probe_and_commit( stream_profile profile, bool zero_copy, frame_callback callback, int buffers) { if(!zero_copy) - buffers = 1; + buffers = 3; if(!_is_capturing && !_callback) { diff --git a/src/media/playback/playback_device.cpp b/src/media/playback/playback_device.cpp index 8e8c2ce12d..4a9a261ac9 100644 --- a/src/media/playback/playback_device.cpp +++ b/src/media/playback/playback_device.cpp @@ -189,7 +189,8 @@ std::shared_ptr playback_device::create_matcher(const frame_holder& fra //TOOD: Use future implementation of matcher factory with the device's name (or other unique identifier) LOG_WARNING("Playback device does not provide a matcher"); auto s = frame.frame->get_stream(); - return std::make_shared(s->get_unique_id(), s->get_stream_type()); + return std::make_shared(s->get_unique_id(), s->get_stream_type(), + generate_matcher_name(s->get_stream_type(), s->get_unique_id())); } void playback_device::set_frame_rate(double rate) diff --git a/src/sync.cpp b/src/sync.cpp index e698bcd42d..5f10d20cec 100644 --- a/src/sync.cpp +++ b/src/sync.cpp @@ -20,9 +20,9 @@ namespace librealsense for (int i = 0; i < composite->get_embedded_frames_count(); i++) { auto matched = composite->get_frame(i); - ss << matched->get_stream()->get_stream_type() << " " << matched->get_frame_number() << ", "<get_frame_timestamp()<<" "; + ss << matched->get_stream()->get_stream_type() << " " << matched->get_frame_number() << ", "<get_frame_timestamp()<<"\n"; } - + //std::cout<( new internal_frame_processor_callback(f))); } - matcher::matcher() - {} - void matcher::set_callback(sync_callback f) { _callback = f; @@ -58,30 +57,36 @@ namespace librealsense _callback(std::move(f), env); } - identity_matcher::identity_matcher(stream_id stream, rs2_stream stream_type) + identity_matcher::identity_matcher(stream_id stream, rs2_stream stream_type, std::string name) { - _stream_id = { stream }; + _name = name; + _streams_id = { stream }; _stream_type = { stream_type }; } void identity_matcher::dispatch(frame_holder f, syncronization_environment env) { + //std::cout <<_name<<"--> "<< f->get_stream()->get_stream_type() << " " << f->get_frame_number() << ", "<get_frame_timestamp()<<"\n"; sync(std::move(f), env); } const std::vector& identity_matcher::get_streams() const { - return _stream_id; + return _streams_id; } const std::vector& identity_matcher::get_streams_types() const { return _stream_type; } - composite_matcher::composite_matcher(std::vector> matchers) + composite_matcher::composite_matcher(std::vector> matchers, std::string name) { + std::stringstream s; + s<get_name()<<" "; for (auto&& stream : matcher->get_streams()) { matcher->set_callback([&](frame_holder f, syncronization_environment env) @@ -96,6 +101,20 @@ namespace librealsense _streams_type.push_back(stream); } } + + _name = s.str(); + + } + + void composite_matcher::dispatch(frame_holder f, syncronization_environment env) + { + //std::cout <<"DISPATCH "<<_name<<"--> "<< f->get_stream()->get_stream_type() << " " << f->get_frame_number() << ", "<get_frame_timestamp()<<"\n"; + + clean_dead_streams(f); + auto matcher = find_matcher(f); + update_last_arrived(f, matcher.get()); + matcher->dispatch(std::move(f), env); + } std::shared_ptr composite_matcher::find_matcher(const frame_holder& frame) @@ -128,6 +147,7 @@ namespace librealsense { matcher = dev->create_matcher(frame); + matcher->set_callback([&](frame_holder f, syncronization_environment env) { sync(std::move(f), env); @@ -153,6 +173,13 @@ namespace librealsense LOG_ERROR("Stream matcher not found! stream=" << rs2_stream_to_string(stream_type)); } } + + else if(!matcher->get_active()) + { + + matcher->set_active(true); + _frames_queue[matcher.get()].start(); + } } } @@ -166,7 +193,7 @@ namespace librealsense { _frames_queue.erase(_matchers[stream_id].get()); } - _matchers[stream_id] = std::make_shared(stream_id, stream_type); + _matchers[stream_id] = std::make_shared(stream_id, stream_type, generate_matcher_name(stream_type, stream_id)); _streams.push_back(stream_id); _streams_type.push_back(stream_type); matcher = _matchers[stream_id]; @@ -182,6 +209,9 @@ namespace librealsense void composite_matcher::sync(frame_holder f, syncronization_environment env) { + std::stringstream s; + s <<"SYNC "<<_name<<"--> "<< f->get_stream()->get_stream_type() << " " << f->get_frame_number() << ", "<get_frame_timestamp()<<"\n"; + // std::cout< match; match.reserve(synced_frames.size()); @@ -297,10 +327,13 @@ namespace librealsense } s<<"\n"; + //std::cout<allocate_composite_frame(std::move(match)); if (composite.frame) { + s <<"SYNCED "<<_name<<"--> "<< composite->get_stream()->get_stream_type() << " " << composite->get_frame_number() << ", "<get_frame_timestamp()<<"\n"; + auto cb = begin_callback(); _callback(std::move(composite), env); } @@ -319,17 +352,13 @@ namespace librealsense } frame_number_composite_matcher::frame_number_composite_matcher(std::vector> matchers) - :composite_matcher(matchers) + :composite_matcher(matchers, "FN: ") { } - void frame_number_composite_matcher::dispatch(frame_holder f, syncronization_environment env) + void frame_number_composite_matcher::update_last_arrived(frame_holder& f, matcher* m) { - auto f1 = f.clone(); - auto matcher = find_matcher(f); - _last_arrived[matcher.get()] =f->get_frame_number(); - matcher->dispatch(std::move(f), env); - clean_dead_streams(f1); + _last_arrived[m] =f->get_frame_number(); } bool frame_number_composite_matcher::are_equivalent(frame_holder& a, frame_holder& b) @@ -348,14 +377,13 @@ namespace librealsense if(_last_arrived[m.second.get()] && (f->get_frame_number() - _last_arrived[m.second.get()]) > 5) { dead_matchers.push_back(m.first); + m.second->set_active(false); } } for(auto id: dead_matchers) { _frames_queue[_matchers[id].get()].clear(); - _frames_queue.erase(_matchers[id].get()); - _matchers.erase(id); } } @@ -363,6 +391,9 @@ namespace librealsense { frame_holder* synced_frame; + if(!missing->get_active()) + return true; + _frames_queue[synced[0]].peek(&synced_frame); auto next_expected = _next_expected[missing]; @@ -392,7 +423,7 @@ namespace librealsense } timestamp_composite_matcher::timestamp_composite_matcher(std::vector> matchers) - :composite_matcher(matchers) + :composite_matcher(matchers, "TS: ") { } bool timestamp_composite_matcher::are_equivalent(frame_holder & a, frame_holder & b) @@ -419,12 +450,9 @@ namespace librealsense return ts.first < ts.second; } - void timestamp_composite_matcher::dispatch(frame_holder f, syncronization_environment env) + void timestamp_composite_matcher::update_last_arrived(frame_holder& f, matcher* m) { - auto matcher = find_matcher(f); - _last_arrived[matcher.get()] = std::chrono::duration(std::chrono::system_clock::now().time_since_epoch()).count(); - matcher->dispatch(std::move(f), env); - clean_dead_streams(f); + _last_arrived[m] = std::chrono::duration(std::chrono::system_clock::now().time_since_epoch()).count(); } void timestamp_composite_matcher::update_next_expected(const frame_holder & f) @@ -447,6 +475,7 @@ namespace librealsense if(_last_arrived[m.second.get()] && (now - _last_arrived[m.second.get()]) > 500) { dead_matchers.push_back(m.first); + m.second->set_active(false); } } @@ -454,12 +483,14 @@ namespace librealsense { _frames_queue[_matchers[id].get()].clear(); _frames_queue.erase(_matchers[id].get()); - _matchers.erase(id); } } bool timestamp_composite_matcher::skip_missing_stream(std::vector synced, matcher* missing) { + if(!missing->get_active()) + return true; + frame_holder* synced_frame; _frames_queue[synced[0]].peek(&synced_frame); diff --git a/src/sync.h b/src/sync.h index 9d6a4b1b35..9544f82e9c 100644 --- a/src/sync.h +++ b/src/sync.h @@ -2,6 +2,10 @@ // Copyright(c) 2015 Intel Corporation. All Rights Reserved. #pragma once + +#ifndef SYNC_H +#define SYNC_H + #include "types.h" #include "archive.h" @@ -14,6 +18,14 @@ namespace librealsense { + typedef int stream_id; + static std::string generate_matcher_name(rs2_stream stream_type, stream_id stream) + { + std::stringstream s; + s< class internal_frame_processor_callback : public rs2_frame_processor_callback { @@ -74,14 +86,12 @@ namespace librealsense }; - typedef int stream_id; + typedef std::function sync_callback; class matcher { public: - matcher(); - virtual void dispatch(frame_holder f, syncronization_environment env) = 0; virtual void sync(frame_holder f, syncronization_environment env); virtual void set_callback(sync_callback f); @@ -105,35 +115,58 @@ namespace librealsense // wait until user is done with all the stuff he chose to borrow _callback_inflight.wait_until_empty(); } + std::string get_name() + { + return _name; + } + + bool get_active() const + { + return _active; + } + + void set_active(const bool active) + { + _active = active; + } + protected: + std::vector _streams_id; + sync_callback _callback; callbacks_heap _callback_inflight; + std::string _name; + + bool _active = true; }; class identity_matcher : public matcher { public: - identity_matcher(stream_id stream, rs2_stream streams_type); + identity_matcher(stream_id stream, rs2_stream streams_type, std::string name); void dispatch(frame_holder f, syncronization_environment env) override; const std::vector& get_streams() const override; const std::vector& get_streams_types() const override; private: - std::vector _stream_id; + std::vector _stream_type; }; class composite_matcher : public matcher { public: - composite_matcher(std::vector> matchers); + composite_matcher(std::vector> matchers, std::string name); virtual bool are_equivalent(frame_holder& a, frame_holder& b) { return true; }; virtual bool is_smaller_than(frame_holder& a, frame_holder& b) { return true; }; virtual bool skip_missing_stream(std::vector synced, matcher* missing) { return false; }; virtual void clean_dead_streams(frame_holder& f) = 0; + virtual void update_last_arrived(frame_holder& f, matcher* m) = 0; + + void dispatch(frame_holder f, syncronization_environment env) override; void sync(frame_holder f, syncronization_environment env) override; const std::vector& get_streams() const override; const std::vector& get_streams_types() const override; @@ -157,7 +190,7 @@ namespace librealsense { public: frame_number_composite_matcher(std::vector> matchers); - void dispatch(frame_holder f, syncronization_environment env) override; + virtual void update_last_arrived(frame_holder& f, matcher* m) override; bool are_equivalent(frame_holder& a, frame_holder& b) override; bool is_smaller_than(frame_holder& a, frame_holder& b) override; bool skip_missing_stream(std::vector synced, matcher* missing) override; @@ -174,7 +207,7 @@ namespace librealsense timestamp_composite_matcher(std::vector> matchers); bool are_equivalent(frame_holder& a, frame_holder& b) override; bool is_smaller_than(frame_holder& a, frame_holder& b) override; - void dispatch(frame_holder f, syncronization_environment env) override; + virtual void update_last_arrived(frame_holder& f, matcher* m) override; void clean_dead_streams(frame_holder& f) override; bool skip_missing_stream(std::vector synced, matcher* missing) override; void update_next_expected(const frame_holder & f) override; @@ -200,3 +233,4 @@ namespace librealsense }; } +#endif diff --git a/tools/realsense-viewer/realsense-viewer.cpp b/tools/realsense-viewer/realsense-viewer.cpp index c08b7c69ff..6f8ea42ebc 100644 --- a/tools/realsense-viewer/realsense-viewer.cpp +++ b/tools/realsense-viewer/realsense-viewer.cpp @@ -36,7 +36,7 @@ void add_playback_device(context& ctx, std::vector& device_models, if (auto p = dev.as()) { auto filename = p.file_name(); - p.set_status_changed_callback([&device_models, filename](rs2_playback_status status) + p.set_status_changed_callback([&viewer_model, &device_models, filename](rs2_playback_status status) { if (status == RS2_PLAYBACK_STATUS_STOPPED) { @@ -52,14 +52,14 @@ void add_playback_device(context& ctx, std::vector& device_models, if (it->_playback_repeat) { //Calling from different since playback callback is from reading thread - std::thread{ [subs]() + std::thread{ [subs, &viewer_model]() { for (auto&& sub : subs) { if (sub->streaming) { auto profiles = sub->get_selected_profiles(); - sub->play(profiles); + sub->play(profiles, viewer_model); } } } }.detach(); @@ -129,7 +129,7 @@ void refresh_devices(std::mutex& m, auto dev_model_itr = std::find_if(begin(device_models), end(device_models), [&](const device_model& other) { return get_device_name(other.dev) == get_device_name(dev); }); - + if (dev_model_itr != end(device_models)) { for (auto&& s : dev_model_itr->subdevices) @@ -143,7 +143,7 @@ void refresh_devices(std::mutex& m, dev_itr = current_connected_devices.erase(dev_itr); continue; - + } ++dev_itr; } @@ -451,7 +451,7 @@ int main(int argv, const char** argc) try if (ImGui::Button(label.c_str(), { 30,30 })) { auto profiles = sub->get_selected_profiles(); - sub->play(profiles); + sub->play(profiles, viewer_model); for (auto&& profile : profiles) { @@ -552,9 +552,13 @@ int main(int argv, const char** argc) try ImGui::PopStyleVar(); ImGui::PopStyleColor(); + frameset f; + if(viewer_model.s.poll_for_frames(&f)) + viewer_model.syncer_queue.enqueue(std::move(f)); + // Fetch frames from queues for (auto&& device_model : device_models) - for (auto&& sub : device_model.subdevices) + for (auto&& sub : device_model.subdevices) { sub->queues.foreach([&](frame_queue& queue) { @@ -563,7 +567,7 @@ int main(int argv, const char** argc) try frame f; if (queue.poll_for_frame(&f)) { - viewer_model.upload_frame(std::move(f)); + viewer_model.upload_frame(std::move(f)); } } catch (const error& ex) From 49b48fc4fdc02b9fab5abb3b3ad0e85e0011cd01 Mon Sep 17 00:00:00 2001 From: aangerma Date: Wed, 8 Nov 2017 10:17:49 +0200 Subject: [PATCH 11/43] start polling frames only after stream started. --- common/model-views.cpp | 40 ++++++++++++++++++++++------------------ common/model-views.h | 12 ++++++++++-- 2 files changed, 32 insertions(+), 20 deletions(-) diff --git a/common/model-views.cpp b/common/model-views.cpp index a409fe8d93..76f0cd852a 100644 --- a/common/model-views.cpp +++ b/common/model-views.cpp @@ -1115,6 +1115,7 @@ namespace rs2 void subdevice_model::play(const std::vector& profiles, viewer_model& viewer) { + viewer.pc.start(); s->open(profiles); try { s->start([&](frame f) { @@ -2658,34 +2659,37 @@ namespace rs2 { while (keep_calculating_pointcloud) { - try + if(streaming) { - if(viewer.syncronize) - { - frameset frames; - if (viewer.syncer_queue.poll_for_frame(&frames)) - { - processing_block.invoke(frames); - } - } - else + try { - std::lock_guard lock(viewer.streams_mutex); - for (auto&& s : viewer.streams) + if(viewer.syncronize) { - if(s.first == viewer.selected_tex_source_uid) + frameset frames; + if (viewer.syncer_queue.poll_for_frame(&frames)) { - update_texture(s.second.texture->get_last_frame()); - last_tex_frame = s.second.texture->get_last_frame(); + processing_block.invoke(frames); } - if(s.first == viewer.selected_depth_source_uid) + } + else + { + std::lock_guard lock(viewer.streams_mutex); + for (auto&& s : viewer.streams) { - processing_block.invoke(s.second.texture->get_last_frame()); + if(s.first == viewer.selected_tex_source_uid) + { + update_texture(s.second.texture->get_last_frame()); + last_tex_frame = s.second.texture->get_last_frame(); + } + if(s.first == viewer.selected_depth_source_uid) + { + processing_block.invoke(s.second.texture->get_last_frame()); + } } } } + catch (...) {} } - catch (...) {} // There is no practical reason to re-calculate the 3D model // at higher frequency then 100 FPS diff --git a/common/model-views.h b/common/model-views.h index 0f062797a8..d1163eb7bb 100644 --- a/common/model-views.h +++ b/common/model-views.h @@ -490,8 +490,9 @@ namespace rs2 }), viewer(viewer), keep_calculating_pointcloud(true), - resulting_3d_models(1), - t([this]() {render_loop(); }) + streaming(false), + resulting_3d_models(1), + t([this]() {render_loop(); }) { processing_block.start(resulting_3d_models); } @@ -526,6 +527,10 @@ namespace rs2 model = f; while (resulting_3d_models.poll_for_frame(&f)); } + void start() + { + streaming = true; + } private: viewer_model& viewer; @@ -537,6 +542,8 @@ namespace rs2 pointcloud pc; rs2::frameset model; std::atomic keep_calculating_pointcloud; + std::atomic streaming; + frame_queue resulting_3d_models; std::thread t; @@ -610,6 +617,7 @@ namespace rs2 bool is_3d_view = false; bool paused = false; + void draw_viewport(const rect& viewer_rect, ux_window& window, int devices, std::string& error_message); bool allow_3d_source_change = true; From 3415cb69346d4b9102775129f67f8c97cbe67f8e Mon Sep 17 00:00:00 2001 From: aangerma Date: Wed, 8 Nov 2017 14:40:44 +0200 Subject: [PATCH 12/43] Fixes bug in python wrraper --- wrappers/python/python.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/wrappers/python/python.cpp b/wrappers/python/python.cpp index d6de0d4e76..2007a4a591 100644 --- a/wrappers/python/python.cpp +++ b/wrappers/python/python.cpp @@ -437,7 +437,7 @@ PYBIND11_PLUGIN(NAME) { /* rs2_processing.hpp */ // Not binding frame_processor_callback, templated - py::class_ processing_block(m, "processing_block"); + py::class_ processing_block(m, "processing_block"); processing_block.def("start", [](rs2::processing_block& self, std::function f) { self.start(f); @@ -567,7 +567,7 @@ PYBIND11_PLUGIN(NAME) { // not binding notifications_callback, templated - py::class_ sensor(m, "sensor"); + py::class_ sensor(m, "sensor"); sensor.def("open", (void (rs2::sensor::*)(const rs2::stream_profile&) const) &rs2::sensor::open, "Open subdevice for exclusive access, by commiting to a configuration", "profile"_a) .def("supports", (bool (rs2::sensor::*)(rs2_camera_info) const) &rs2::device::supports, From 52251968c1dcd96d12f40ce76fd6e856f0b7cea0 Mon Sep 17 00:00:00 2001 From: "Shao,Ting" Date: Tue, 7 Nov 2017 15:11:21 +0800 Subject: [PATCH 13/43] [Node.js] Add Options API to colorizer and new options --- wrappers/nodejs/index.js | 76 +++++++- wrappers/nodejs/src/addon.cpp | 229 ++++++++++++++++-------- wrappers/nodejs/test/test-functional.js | 29 ++- 3 files changed, 251 insertions(+), 83 deletions(-) diff --git a/wrappers/nodejs/index.js b/wrappers/nodejs/index.js index 6f9a572ea3..c0f0d74134 100644 --- a/wrappers/nodejs/index.js +++ b/wrappers/nodejs/index.js @@ -386,6 +386,10 @@ class Options { constructor(cxxObj) { this.cxxObj = cxxObj; } + + setCxxOptionsObject(cxxObj) { + this.cxxObj = cxxObj; + } /** * Check if particular option is read-only * @param {String|Number} option The option to be checked @@ -1147,10 +1151,12 @@ class PointCloud { /** * The Colorizer can be used to quickly visualize the depth data by tranform data into RGB8 format */ -class Colorizer { +class Colorizer extends Options { constructor() { + super(); this.cxxColorizer = new RS2.RSColorizer(); this.cxxColorizer.create(); + this.setCxxOptionsObject(this.cxxColorizer); this.depthRGB = new VideoFrame(); } @@ -3238,7 +3244,36 @@ const option = { * correction of the motion data .
Equivalent to its uppercase counterpart. */ option_enable_motion_correction: 'enable-motion-correction', + /** + * String literal of 'auto-exposure-priority'. + *
Allows sensor to dynamically ajust the frame rate + * depending on lighting conditions
Equivalent to its uppercase counterpart + */ + option_auto_exposure_priority: 'auto-exposure-priority', + /** + * String literal of ''color-scheme'.
Color scheme for data visualization + *
Equivalent to its uppercase counterpart + */ + option_color_scheme: 'color-scheme', + + /** + * String literal of ''histogram-equalization-enabled'.
Perform histogram + * equalization post-processing on the depth data
Equivalent to its uppercase counterpart + */ + option_histogram_equalization_enabled: 'histogram-equalization-enabled', + + /** + * String literal of ''min-distance'.
Minimal distance to the target + *
Equivalent to its uppercase counterpart + */ + option_min_distance: 'min-distance', + + /** + * String literal of ''max-distance'.
Maximum distance to the target + *
Equivalent to its uppercase counterpart + */ + option_max_distance: 'max-distance', /** * Enable / disable color backlight compensatio.
Equivalent to its lowercase counterpart. * @type {Integer} @@ -3406,6 +3441,32 @@ const option = { * @type {Integer} */ OPTION_ENABLE_MOTION_CORRECTION: RS2.RS2_OPTION_ENABLE_MOTION_CORRECTION, + /** + * Allows sensor to dynamically ajust the frame rate depending on lighting conditions. + *
Equivalent to its uppercase counterpart + */ + OPTION_AUTO_EXPOSURE_PRIORITY: RS2.OPTION_AUTO_EXPOSURE_PRIORITY, + + /** + * Color scheme for data visualization
Equivalent to its uppercase counterpart + */ + OPTION_COLOR_SCHEME: RS2.OPTION_COLOR_SCHEME, + + /** + * Perform histogram equalization post-processing on the depth data. + *
Equivalent to its uppercase counterpart + */ + OPTION_HISTOGRAM_EQUALIZATION_ENABLED: RS2.OPTION_HISTOGRAM_EQUALIZATION_ENABLED, + + /** + * Minimal distance to the target
Equivalent to its uppercase counterpart + */ + OPTION_MIN_DISTANCE: RS2.OPTION_MIN_DISTANCE, + + /** + * Maximum distance to the target
Equivalent to its uppercase counterpart + */ + OPTION_MAX_DISTANCE: RS2.OPTION_MAX_DISTANCE, /** * Number of enumeration values. Not a valid input: intended to be used in for-loops. * @type {Integer} @@ -3487,6 +3548,19 @@ const option = { return this.option_depth_units; case this.OPTION_ENABLE_MOTION_CORRECTION: return this.option_enable_motion_correction; + case this.OPTION_AUTO_EXPOSURE_PRIORITY: + return this.option_auto_exposure_priority; + case this.OPTION_COLOR_SCHEME: + return this.option_color_scheme; + case this.OPTION_HISTOGRAM_EQUALIZATION_ENABLED: + return this.option_histogram_equalization_enabled; + case this.OPTION_MIN_DISTANCE: + return this.option_min_distance; + case this.OPTION_MAX_DISTANCE: + return this.option_max_distance; + default: + throw new TypeError( + 'option.optionToString(option) expects a valid value as the 1st argument'); } } }, diff --git a/wrappers/nodejs/src/addon.cpp b/wrappers/nodejs/src/addon.cpp index 9cb1d8f2b3..ed23e4b2a3 100644 --- a/wrappers/nodejs/src/addon.cpp +++ b/wrappers/nodejs/src/addon.cpp @@ -1455,47 +1455,87 @@ Nan::Persistent RSSyncer::constructor_; class Options { public: - explicit Options(rs2_options* options) : options_(options), error_(nullptr) {} + Options() : error_(nullptr) {} - ~Options() { + virtual ~Options() { if (error_) rs2_free_error(error_); } - bool SupportsOption(rs2_option option) { - return (rs2_supports_option(options_, option, &error_) != 0); - } + virtual rs2_options* GetOptionsPointer() = 0; - float GetOption(rs2_option option) { - return rs2_get_option(options_, option, &error_); + void SupportsOptionInternal( + const Nan::FunctionCallbackInfo& info) { + int32_t option = info[0]->IntegerValue(); + auto on = rs2_supports_option( + GetOptionsPointer(), static_cast(option), &error_); + info.GetReturnValue().Set(Nan::New(on ? true : false)); + return; } - const char* GetOptionDescription(rs2_option option) { - return rs2_get_option_description(options_, option, &error_); + void GetOptionInternal(const Nan::FunctionCallbackInfo& info) { + int32_t option = info[0]->IntegerValue(); + auto value = rs2_get_option( + GetOptionsPointer(), static_cast(option), &error_); + info.GetReturnValue().Set(Nan::New(value)); } - const char* GetOptionValueDescription(rs2_option option, float val) { - return rs2_get_option_value_description(options_, option, val, &error_); + void GetOptionDescriptionInternal( + const Nan::FunctionCallbackInfo& info) { + int32_t option = info[0]->IntegerValue(); + auto desc = rs2_get_option_description( + GetOptionsPointer(), static_cast(option), &error_); + if (desc) + info.GetReturnValue().Set(Nan::New(desc).ToLocalChecked()); + else + info.GetReturnValue().Set(Nan::Undefined()); } - void SetOption(rs2_option option, float val) { - rs2_set_option(options_, option, val, &error_); + void GetOptionValueDescriptionInternal( + const Nan::FunctionCallbackInfo& info) { + int32_t option = info[0]->IntegerValue(); + auto val = info[1]->NumberValue(); + auto desc = rs2_get_option_value_description( + GetOptionsPointer(), static_cast(option), val, &error_); + if (desc) + info.GetReturnValue().Set(Nan::New(desc).ToLocalChecked()); + else + info.GetReturnValue().Set(Nan::Undefined()); } - void GetOptionRange( - rs2_option option, float* min, float* max, float* step, float* def) { - rs2_get_option_range(options_, option, min, max, step, def, &error_); + void SetOptionInternal(const Nan::FunctionCallbackInfo& info) { + int32_t option = info[0]->IntegerValue(); + auto val = info[1]->NumberValue(); + rs2_set_option( + GetOptionsPointer(), static_cast(option), val, &error_); + info.GetReturnValue().Set(Nan::Undefined()); } - bool IsOptionReadonly(rs2_option option) { - return (rs2_is_option_read_only(options_, option, &error_) != 0); + void GetOptionRangeInternal( + const Nan::FunctionCallbackInfo& info) { + int32_t option = info[0]->IntegerValue(); + float min = 0; + float max = 0; + float step = 0; + float def = 0; + rs2_get_option_range( + GetOptionsPointer(), static_cast(option), &min, &max, &step, + &def, &error_); + info.GetReturnValue().Set(RSOptionRange(min, max, step, def).GetObject()); + } + + void IsOptionReadonlyInternal( + const Nan::FunctionCallbackInfo& info) { + int32_t option = info[0]->IntegerValue(); + auto val = rs2_is_option_read_only( + GetOptionsPointer(), static_cast(option), &error_); + info.GetReturnValue().Set(Nan::New((val) ? true : false)); } private: - rs2_options* options_; rs2_error* error_; }; -class RSSensor : public Nan::ObjectWrap { +class RSSensor : public Nan::ObjectWrap, Options { public: static void Init(v8::Local exports) { v8::Local tpl = Nan::New(New); @@ -1544,10 +1584,15 @@ class RSSensor : public Nan::ObjectWrap { auto me = Nan::ObjectWrap::Unwrap(instance); me->sensor_ = sensor; - me->options_ = new Options(reinterpret_cast(sensor)); return scope.Escape(instance); } + rs2_options* GetOptionsPointer() override { + // TODO(shaoting) find better way to avoid the reinterpret_cast which was + // caused the inheritance relation was hidden + return reinterpret_cast(sensor_); + } + void ReplaceFrame(rs2_frame* raw_frame) { // clear old frame first. frame_->Replace(nullptr); @@ -1567,8 +1612,7 @@ class RSSensor : public Nan::ObjectWrap { private: RSSensor() : sensor_(nullptr), error_(nullptr), profile_list_(nullptr), - frame_(nullptr), video_frame_(nullptr), depth_frame_(nullptr), - options_(nullptr) {} + frame_(nullptr), video_frame_(nullptr), depth_frame_(nullptr) {} ~RSSensor() { DestroyMe(); @@ -1583,8 +1627,6 @@ class RSSensor : public Nan::ObjectWrap { sensor_ = nullptr; if (profile_list_) rs2_delete_stream_profiles_list(profile_list_); profile_list_ = nullptr; - if (options_) delete options_; - options_ = nullptr; } static void New(const Nan::FunctionCallbackInfo& info) { @@ -1596,89 +1638,51 @@ class RSSensor : public Nan::ObjectWrap { } static NAN_METHOD(SupportsOption) { - int32_t option = info[0]->IntegerValue(); auto me = Nan::ObjectWrap::Unwrap(info.Holder()); - if (me) { - auto on = me->options_->SupportsOption(static_cast(option)); - info.GetReturnValue().Set(Nan::New(on ? true : false)); - return; - } + if (me) return me->SupportsOptionInternal(info); + info.GetReturnValue().Set(Nan::False()); } static NAN_METHOD(GetOption) { - int32_t option = info[0]->IntegerValue(); auto me = Nan::ObjectWrap::Unwrap(info.Holder()); - if (me) { - auto value = me->options_->GetOption(static_cast(option)); - info.GetReturnValue().Set(Nan::New(value)); - return; - } + if (me) return me->GetOptionInternal(info); + info.GetReturnValue().Set(Nan::Undefined()); } static NAN_METHOD(GetOptionDescription) { - int32_t option = info[0]->IntegerValue(); auto me = Nan::ObjectWrap::Unwrap(info.Holder()); - if (me) { - auto desc = me->options_->GetOptionDescription(static_cast( - option)); - info.GetReturnValue().Set(Nan::New(desc).ToLocalChecked()); - return; - } + if (me) return me->GetOptionDescriptionInternal(info); + info.GetReturnValue().Set(Nan::Undefined()); } static NAN_METHOD(GetOptionValueDescription) { - int32_t option = info[0]->IntegerValue(); - auto val = info[1]->NumberValue(); auto me = Nan::ObjectWrap::Unwrap(info.Holder()); - if (me) { - auto desc = me->options_->GetOptionValueDescription( - static_cast(option), val); - if (desc) { - info.GetReturnValue().Set(Nan::New(desc).ToLocalChecked()); - return; - } - } + if (me) return me->GetOptionValueDescriptionInternal(info); + info.GetReturnValue().Set(Nan::Undefined()); } static NAN_METHOD(SetOption) { - int32_t option = info[0]->IntegerValue(); - auto value = info[1]->NumberValue(); auto me = Nan::ObjectWrap::Unwrap(info.Holder()); - if (me) { - me->options_->SetOption(static_cast(option), value); - } + if (me) return me->SetOptionInternal(info); + info.GetReturnValue().Set(Nan::Undefined()); } static NAN_METHOD(GetOptionRange) { - int32_t option = info[0]->IntegerValue(); auto me = Nan::ObjectWrap::Unwrap(info.Holder()); - if (me) { - float min = 0; - float max = 0; - float step = 0; - float def = 0; - me->options_->GetOptionRange(static_cast(option), &min, &max, - &step, &def); - info.GetReturnValue().Set(RSOptionRange(min, max, step, def).GetObject()); - return; - } + if (me) return me->GetOptionRangeInternal(info); + info.GetReturnValue().Set(Nan::Undefined()); } static NAN_METHOD(IsOptionReadonly) { - int32_t option = info[0]->IntegerValue(); auto me = Nan::ObjectWrap::Unwrap(info.Holder()); - if (me) { - auto val = me->options_->IsOptionReadonly( - static_cast(option)); - info.GetReturnValue().Set(Nan::New((val) ? true : false)); - return; - } + if (me) return me->IsOptionReadonlyInternal(info); + info.GetReturnValue().Set(Nan::False()); } @@ -1898,7 +1902,6 @@ class RSSensor : public Nan::ObjectWrap { RSFrame* frame_; RSFrame* video_frame_; RSFrame* depth_frame_; - Options* options_; friend class RSContext; friend class DevicesChangedCallbackInfo; friend class FrameCallbackInfo; @@ -3080,7 +3083,7 @@ bool RSConfig::CanResolveInternal(rs2_config* config, return rs2_config_can_resolve(config, pipe->pipeline_, error); } -class RSColorizer : public Nan::ObjectWrap { +class RSColorizer : public Nan::ObjectWrap, Options { public: static void Init(v8::Local exports) { v8::Local tpl = Nan::New(New); @@ -3090,6 +3093,14 @@ class RSColorizer : public Nan::ObjectWrap { Nan::SetPrototypeMethod(tpl, "destroy", Destroy); Nan::SetPrototypeMethod(tpl, "create", Create); Nan::SetPrototypeMethod(tpl, "colorize", Colorize); + Nan::SetPrototypeMethod(tpl, "supportsOption", SupportsOption); + Nan::SetPrototypeMethod(tpl, "getOption", GetOption); + Nan::SetPrototypeMethod(tpl, "setOption", SetOption); + Nan::SetPrototypeMethod(tpl, "getOptionRange", GetOptionRange); + Nan::SetPrototypeMethod(tpl, "isOptionReadonly", IsOptionReadonly); + Nan::SetPrototypeMethod(tpl, "getOptionDescription", GetOptionDescription); + Nan::SetPrototypeMethod(tpl, "getOptionValueDescription", + GetOptionValueDescription); constructor_.Reset(tpl->GetFunction()); exports->Set(Nan::New("RSColorizer").ToLocalChecked(), tpl->GetFunction()); @@ -3108,6 +3119,12 @@ class RSColorizer : public Nan::ObjectWrap { return scope.Escape(instance); } + rs2_options* GetOptionsPointer() override { + // TODO(shaoting) find better way to avoid the reinterpret_cast which was + // caused the inheritance relation was hidden + return reinterpret_cast(colorizer_); + } + private: RSColorizer() : colorizer_(nullptr), frame_queue_(nullptr), error_(nullptr) {} @@ -3171,6 +3188,55 @@ class RSColorizer : public Nan::ObjectWrap { info.GetReturnValue().Set(Nan::False()); } + static NAN_METHOD(SupportsOption) { + auto me = Nan::ObjectWrap::Unwrap(info.Holder()); + if (me) return me->SupportsOptionInternal(info); + + info.GetReturnValue().Set(Nan::False()); + } + + static NAN_METHOD(GetOption) { + auto me = Nan::ObjectWrap::Unwrap(info.Holder()); + if (me) return me->GetOptionInternal(info); + + info.GetReturnValue().Set(Nan::Undefined()); + } + + static NAN_METHOD(GetOptionDescription) { + auto me = Nan::ObjectWrap::Unwrap(info.Holder()); + if (me) return me->GetOptionDescriptionInternal(info); + + info.GetReturnValue().Set(Nan::Undefined()); + } + + static NAN_METHOD(GetOptionValueDescription) { + auto me = Nan::ObjectWrap::Unwrap(info.Holder()); + if (me) return me->GetOptionValueDescriptionInternal(info); + + info.GetReturnValue().Set(Nan::Undefined()); + } + + static NAN_METHOD(SetOption) { + auto me = Nan::ObjectWrap::Unwrap(info.Holder()); + if (me) return me->SetOptionInternal(info); + + info.GetReturnValue().Set(Nan::Undefined()); + } + + static NAN_METHOD(GetOptionRange) { + auto me = Nan::ObjectWrap::Unwrap(info.Holder()); + if (me) return me->GetOptionRangeInternal(info); + + info.GetReturnValue().Set(Nan::Undefined()); + } + + static NAN_METHOD(IsOptionReadonly) { + auto me = Nan::ObjectWrap::Unwrap(info.Holder()); + if (me) me->IsOptionReadonlyInternal(info); + + info.GetReturnValue().Set(Nan::False()); + } + private: static Nan::Persistent constructor_; @@ -3432,6 +3498,11 @@ void InitModule(v8::Local exports) { _FORCE_SET_ENUM(RS2_OPTION_MOTION_MODULE_TEMPERATURE); _FORCE_SET_ENUM(RS2_OPTION_DEPTH_UNITS); _FORCE_SET_ENUM(RS2_OPTION_ENABLE_MOTION_CORRECTION); + _FORCE_SET_ENUM(RS2_OPTION_AUTO_EXPOSURE_PRIORITY); + _FORCE_SET_ENUM(RS2_OPTION_COLOR_SCHEME); + _FORCE_SET_ENUM(RS2_OPTION_HISTOGRAM_EQUALIZATION_ENABLED); + _FORCE_SET_ENUM(RS2_OPTION_MIN_DISTANCE); + _FORCE_SET_ENUM(RS2_OPTION_MAX_DISTANCE); _FORCE_SET_ENUM(RS2_OPTION_COUNT); // rs2_camera_info diff --git a/wrappers/nodejs/test/test-functional.js b/wrappers/nodejs/test/test-functional.js index 6ede45967c..c98df97d55 100644 --- a/wrappers/nodejs/test/test-functional.js +++ b/wrappers/nodejs/test/test-functional.js @@ -193,6 +193,29 @@ describe('Colorizer test', function() { assert.equal(depthRGB.width, depth.width); assert.equal(depthRGB.format, rs2.format.FORMAT_RGB8); }); + it('colorizer option API test', () => { + for (let i = rs2.option.OPTION_BACKLIGHT_COMPENSATION; i < rs2.option.OPTION_COUNT; i++) { + let readonly = colorizer.isOptionReadOnly(i); + assert.equal((readonly === undefined) || (typeof readonly === 'boolean'), true); + let supports = colorizer.supportsOption(i); + assert.equal((supports === undefined) || (typeof supports === 'boolean'), true); + let value = colorizer.getOption(i); + assert.equal((value === undefined) || (typeof value === 'number'), true); + let des = colorizer.getOptionDescription(i); + assert.equal((des === undefined) || (typeof des === 'string'), true); + des = colorizer.getOptionDescription(i, 1); + assert.equal((des === undefined) || (typeof des === 'string'), true); + + if (supports && !readonly) { + let range = colorizer.getOptionRange(i); + for (let j = range.minvalue; j <= range.maxValue; j += range.step) { + colorizer.setOption(i, j); + let val = colorizer.getOption(i); + assert.equal(val, j); + } + } + } + }); }); @@ -414,9 +437,6 @@ describe('Sensor tests', function() { it('Notification test', () => { return new Promise((resolve, reject) => { let dev = ctx.queryDevices().devices[0]; - setTimeout(() => { - dev.cxxDev.triggerErrorForTest(); - }, 500); sensors[0].setNotificationsCallback((n) => { assert.equal(typeof n.descr, 'string'); assert.equal(typeof n.timestamp, 'number'); @@ -424,6 +444,9 @@ describe('Sensor tests', function() { assert.equal(typeof n.category, 'number'); resolve(); }); + setTimeout(() => { + dev.cxxDev.triggerErrorForTest(); + }, 100); }); }); }); From 427371d6aeee14e15ea33470308467e89bf5075b Mon Sep 17 00:00:00 2001 From: icarpis Date: Thu, 9 Nov 2017 11:16:43 +0200 Subject: [PATCH 14/43] Fixed several bugs --- common/ux-window.cpp | 30 ++++++++++++++++----- common/ux-window.h | 3 +++ examples/C/depth/rs-depth.c | 2 +- src/ds5/advanced_mode/json_loader.hpp | 7 ++++- src/log.cpp | 3 ++- tools/depth-quality/depth-quality-model.cpp | 15 +++++++++-- tools/fw-logger/rs-fw-logger.cpp | 8 +++--- 7 files changed, 53 insertions(+), 15 deletions(-) diff --git a/common/ux-window.cpp b/common/ux-window.cpp index ebc3c4aed4..8394114d2d 100644 --- a/common/ux-window.cpp +++ b/common/ux-window.cpp @@ -14,7 +14,7 @@ namespace rs2 _win(nullptr), _width(0), _height(0), _output_height(0), _font_14(nullptr), _font_18(nullptr), _app_ready(false), _first_frame(true), _query_devices(true), _missing_device(false), - _hourglass_index(0), _dev_stat_message{} + _hourglass_index(0), _dev_stat_message{}, _keep_alive(true) { if (!glfwInit()) exit(1); @@ -97,8 +97,15 @@ namespace rs2 if (_first_frame) { - std::thread first_load([&]() { - do + if (_first_load.joinable()) + { + _keep_alive = false; + _first_load.join(); + _keep_alive = true; + } + + _first_load = std::thread([&]() { + while (_keep_alive && !_app_ready) { try { @@ -108,10 +115,9 @@ namespace rs2 { std::this_thread::sleep_for(std::chrono::seconds(1)); // Wait for connect event and retry } - } while (!_app_ready); + } }); _first_frame = false; - first_load.detach(); } // If we are just getting started, render the Splash Screen instead of normal UI @@ -163,7 +169,7 @@ namespace rs2 ImGui::PushStyleVar(ImGuiStyleVar_WindowPadding, { 5, 5 }); ImGui::PushStyleVar(ImGuiStyleVar_WindowRounding, 1); ImGui::PushStyleColor(ImGuiCol_WindowBg, transparent); - ImGui::SetNextWindowPos({ (float)_width/2 - 150, (float)_height/2 + 70 }); + ImGui::SetNextWindowPos({ (float)_width / 2 - 150, (float)_height / 2 + 70 }); ImGui::PushFont(_font_18); ImGui::SetNextWindowSize({ (float)_width, (float)_height }); ImGui::Begin("Splash Screen Banner", nullptr, flags); @@ -211,6 +217,13 @@ namespace rs2 ux_window::~ux_window() { + if (_first_load.joinable()) + { + _keep_alive = false; + _first_load.join(); + } + + ImGui::GetIO().Fonts->ClearFonts(); // To be refactored into Viewer theme object ImGui_ImplGlfw_Shutdown(); glfwDestroyWindow(_win); @@ -269,5 +282,10 @@ namespace rs2 _app_ready = false; _splash_timer.reset(); _dev_stat_message = u8"\uf287 Please connect Intel RealSense device!"; + + { + std::lock_guard lock(_on_load_message_mtx); + _on_load_message.clear(); + } } } diff --git a/common/ux-window.h b/common/ux-window.h index cbdbbc10e4..b86544af0e 100644 --- a/common/ux-window.h +++ b/common/ux-window.h @@ -6,6 +6,7 @@ #include "imgui.h" #include #include +#include #include "rendering.h" namespace rs2 @@ -71,8 +72,10 @@ namespace rs2 std::string _error_message; float _scale_factor; + std::thread _first_load; bool _first_frame; std::atomic _app_ready; + std::atomic _keep_alive; texture_buffer _splash_tex; timer _splash_timer; std::string _title_str; diff --git a/examples/C/depth/rs-depth.c b/examples/C/depth/rs-depth.c index 48a7b5b960..ea5269d610 100644 --- a/examples/C/depth/rs-depth.c +++ b/examples/C/depth/rs-depth.c @@ -56,7 +56,7 @@ float get_depth_unit_value(const rs2_device* const dev) if (1 == is_depth_sensor_found) { - depth_scale = rs2_get_option(sensor, RS2_OPTION_DEPTH_UNITS, &e); + depth_scale = rs2_get_option((const rs2_sensor*)sensor, RS2_OPTION_DEPTH_UNITS, &e); check_error(e); rs2_delete_sensor(sensor); break; diff --git a/src/ds5/advanced_mode/json_loader.hpp b/src/ds5/advanced_mode/json_loader.hpp index 7f07f8d904..fd8062abe2 100644 --- a/src/ds5/advanced_mode/json_loader.hpp +++ b/src/ds5/advanced_mode/json_loader.hpp @@ -373,7 +373,6 @@ namespace librealsense { "aux-param-depthclampmin", make_field(p.depth_table, &STDepthTableControl::depthClampMin) }, { "aux-param-depthclampmax", make_field(p.depth_table, &STDepthTableControl::depthClampMax) }, { "param-disparitymode", make_field(p.depth_table, &STDepthTableControl::disparityMode) }, - { "aux-param-disparitymultiplier", make_field(p.depth_table, &STDepthTableControl::disparityMode) }, { "param-disparityshift", make_field(p.depth_table, &STDepthTableControl::disparityShift) }, { "aux-param-disparityshift", make_field(p.depth_table, &STDepthTableControl::disparityShift) }, @@ -392,6 +391,12 @@ namespace librealsense { "param-regionspatialthresholdv", make_ignored_field() }, { "result:", make_ignored_field() }, { "result", make_ignored_field() }, + { "aux-param-disparitymultiplier", make_ignored_field() }, + { "stream-depth-format", make_ignored_field() }, + { "stream-ir-format", make_ignored_field() }, + { "stream-width", make_ignored_field() }, + { "stream-height", make_ignored_field() }, + { "stream-fps", make_ignored_field() }, }; static const std::map auto_control_values{ { "False", 0.f }, { "True", 1.f } }; diff --git a/src/log.cpp b/src/log.cpp index 41c3580004..df9c8b3e5e 100644 --- a/src/log.cpp +++ b/src/log.cpp @@ -99,12 +99,13 @@ namespace librealsense if (content) { std::string content_str(content); - std::transform(content_str.begin(), content_str.end(), content_str.begin(), ::toupper); + std::transform(content_str.begin(), content_str.end(), content_str.begin(), ::tolower); for (uint32_t i = 0; i < RS2_LOG_SEVERITY_COUNT; i++) { auto current = (rs2_log_severity)i; std::string name = librealsense::get_string(current); + std::transform(name.begin(), name.end(), name.begin(), ::tolower); if (content_str == name) { severity = current; diff --git a/tools/depth-quality/depth-quality-model.cpp b/tools/depth-quality/depth-quality-model.cpp index 9ae6bc9a80..ab4f002cbf 100644 --- a/tools/depth-quality/depth-quality-model.cpp +++ b/tools/depth-quality/depth-quality-model.cpp @@ -46,8 +46,19 @@ namespace rs2 { if (valid_config = cfg.can_resolve(_pipe)) { - _pipe.start(cfg); - break; + static bool device_is_used = false; + try { + _pipe.start(cfg); + break; + } + catch (...) + { + if (!device_is_used) + { + window.add_on_load_message("Device is used by another application!"); + device_is_used = true; + } + } } } diff --git a/tools/fw-logger/rs-fw-logger.cpp b/tools/fw-logger/rs-fw-logger.cpp index 90b8fd22e1..60b7fd0dbe 100644 --- a/tools/fw-logger/rs-fw-logger.cpp +++ b/tools/fw-logger/rs-fw-logger.cpp @@ -72,13 +72,13 @@ int main(int argc, char* argv[]) while (true) { - cout << "\nWaiting for RealSense device to connect...\n"; - auto dev = hub.wait_for_device(); - cout << "RealSense device has connected...\n"; try { - vector input; + cout << "\nWaiting for RealSense device to connect...\n"; + auto dev = hub.wait_for_device(); + cout << "RealSense device has connected...\n"; + vector input; auto str_op_code = dev.get_info(RS2_CAMERA_INFO_DEBUG_OP_CODE); auto op_code = static_cast(stoi(str_op_code)); input = {0x14, 0x00, 0xab, 0xcd, op_code, 0x00, 0x00, 0x00, From d470a116ca347b7012f0bb30a15fc8c07bc96233 Mon Sep 17 00:00:00 2001 From: "Shao,Ting" Date: Thu, 9 Nov 2017 15:51:10 +0800 Subject: [PATCH 15/43] [Node.js] Add Node.js version requirement in README.md --- wrappers/nodejs/README.md | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/wrappers/nodejs/README.md b/wrappers/nodejs/README.md index b0cfc1e99b..b5a2c3fd00 100644 --- a/wrappers/nodejs/README.md +++ b/wrappers/nodejs/README.md @@ -6,7 +6,14 @@ This is the Node.js wrapper for the C++ `librealsense2` for Intel® RealSense™ ## 1.1. Install Build Prerequisites ### Install Node.js -Install [`Node.js`](https://nodejs.org/en/download/). +Install [`Node.js`](https://nodejs.org/en/download/), the version should be at least v6.x. + +Note: In Ubuntu16.04, the default apt-get installed version may not meet the requirement. Manually install the required version is necessary. +The version can be checked through this command: + +``` +node -v +``` After `Node.js` is installed, run the following command to install required modules. From 53c8a10da16dad0d2da4b038604a16732c937b75 Mon Sep 17 00:00:00 2001 From: icarpis Date: Thu, 9 Nov 2017 16:05:32 +0200 Subject: [PATCH 16/43] Code review fixes --- common/ux-window.cpp | 15 ++++++++------- examples/C/depth/rs-depth.c | 2 +- tools/depth-quality/depth-quality-model.cpp | 8 ++++---- tools/depth-quality/depth-quality-model.h | 1 + tools/fw-logger/rs-fw-logger.cpp | 2 +- 5 files changed, 15 insertions(+), 13 deletions(-) diff --git a/common/ux-window.cpp b/common/ux-window.cpp index 8394114d2d..cd44cbff12 100644 --- a/common/ux-window.cpp +++ b/common/ux-window.cpp @@ -97,12 +97,7 @@ namespace rs2 if (_first_frame) { - if (_first_load.joinable()) - { - _keep_alive = false; - _first_load.join(); - _keep_alive = true; - } + assert(!_first_load.joinable()); // You must call to reset() before initiate new thread _first_load = std::thread([&]() { while (_keep_alive && !_app_ready) @@ -223,7 +218,6 @@ namespace rs2 _first_load.join(); } - ImGui::GetIO().Fonts->ClearFonts(); // To be refactored into Viewer theme object ImGui_ImplGlfw_Shutdown(); glfwDestroyWindow(_win); @@ -275,6 +269,13 @@ namespace rs2 void ux_window::reset() { + if (_first_load.joinable()) + { + _keep_alive = false; + _first_load.join(); + _keep_alive = true; + } + _query_devices = true; _missing_device = false; _hourglass_index = 0; diff --git a/examples/C/depth/rs-depth.c b/examples/C/depth/rs-depth.c index ea5269d610..3f8d24d6ad 100644 --- a/examples/C/depth/rs-depth.c +++ b/examples/C/depth/rs-depth.c @@ -56,7 +56,7 @@ float get_depth_unit_value(const rs2_device* const dev) if (1 == is_depth_sensor_found) { - depth_scale = rs2_get_option((const rs2_sensor*)sensor, RS2_OPTION_DEPTH_UNITS, &e); + depth_scale = rs2_get_option((const rs2_options*)sensor, RS2_OPTION_DEPTH_UNITS, &e); check_error(e); rs2_delete_sensor(sensor); break; diff --git a/tools/depth-quality/depth-quality-model.cpp b/tools/depth-quality/depth-quality-model.cpp index ab4f002cbf..4cf3189e87 100644 --- a/tools/depth-quality/depth-quality-model.cpp +++ b/tools/depth-quality/depth-quality-model.cpp @@ -46,17 +46,16 @@ namespace rs2 { if (valid_config = cfg.can_resolve(_pipe)) { - static bool device_is_used = false; try { _pipe.start(cfg); break; } catch (...) { - if (!device_is_used) + if (!_device_in_use) { - window.add_on_load_message("Device is used by another application!"); - device_is_used = true; + window.add_on_load_message("Device is not functional or busy!"); + _device_in_use = true; } } } @@ -759,6 +758,7 @@ namespace rs2 } catch(...){} + _device_in_use = false; _first_frame = true; win.reset(); } diff --git a/tools/depth-quality/depth-quality-model.h b/tools/depth-quality/depth-quality-model.h index 89ed013909..050e5ee3f2 100644 --- a/tools/depth-quality/depth-quality-model.h +++ b/tools/depth-quality/depth-quality-model.h @@ -222,6 +222,7 @@ namespace rs2 std::string _error_message; bool _first_frame = true; periodic_timer _update_readonly_options_timer; + bool _device_in_use = false; float _roi_percent = 0.4f; int _roi_combo_index = 2; diff --git a/tools/fw-logger/rs-fw-logger.cpp b/tools/fw-logger/rs-fw-logger.cpp index 60b7fd0dbe..3a2f728e50 100644 --- a/tools/fw-logger/rs-fw-logger.cpp +++ b/tools/fw-logger/rs-fw-logger.cpp @@ -76,7 +76,7 @@ int main(int argc, char* argv[]) { cout << "\nWaiting for RealSense device to connect...\n"; auto dev = hub.wait_for_device(); - cout << "RealSense device has connected...\n"; + cout << "RealSense device was connected...\n"; vector input; auto str_op_code = dev.get_info(RS2_CAMERA_INFO_DEBUG_OP_CODE); From 2526100b21c26a75ab8d4187fd388c8bd8140d9d Mon Sep 17 00:00:00 2001 From: aangerma Date: Thu, 9 Nov 2017 14:10:24 +0200 Subject: [PATCH 17/43] Fixes following code review --- src/device.cpp | 3 +- src/ds5/ds5-color.cpp | 3 +- src/ds5/ds5-device.cpp | 3 +- src/ds5/ds5-motion.cpp | 3 +- src/image.cpp | 2 +- src/ivcam/sr300.cpp | 6 +- src/media/playback/playback_device.cpp | 3 +- src/sync.cpp | 77 ++++++++++++++------ src/sync.h | 79 ++++++++------------- tools/realsense-viewer/realsense-viewer.cpp | 1 - 10 files changed, 94 insertions(+), 86 deletions(-) diff --git a/src/device.cpp b/src/device.cpp index 22a2e02290..8640dd03be 100644 --- a/src/device.cpp +++ b/src/device.cpp @@ -102,8 +102,7 @@ void device::hardware_reset() std::shared_ptr librealsense::device::create_matcher(const frame_holder& frame) const { - return std::make_shared( frame.frame->get_stream()->get_unique_id(), frame.frame->get_stream()->get_stream_type() , - generate_matcher_name(frame.frame->get_stream()->get_stream_type(), frame.frame->get_stream()->get_unique_id())); + return std::make_shared( frame.frame->get_stream()->get_unique_id(), frame.frame->get_stream()->get_stream_type()); } std::pair librealsense::device::get_extrinsics(const stream_interface& stream) const diff --git a/src/ds5/ds5-color.cpp b/src/ds5/ds5-color.cpp index 394ca7bd83..ddce42942f 100644 --- a/src/ds5/ds5-color.cpp +++ b/src/ds5/ds5-color.cpp @@ -171,7 +171,6 @@ namespace librealsense return device::create_matcher(frame); } - return std::make_shared( _color_stream->get_unique_id(), _color_stream->get_stream_type(), - generate_matcher_name(_color_stream->get_stream_type(), _color_stream->get_unique_id())); + return std::make_shared( _color_stream->get_unique_id(), _color_stream->get_stream_type()); } } diff --git a/src/ds5/ds5-device.cpp b/src/ds5/ds5-device.cpp index 69bb56dcf0..d07b28b42e 100644 --- a/src/ds5/ds5-device.cpp +++ b/src/ds5/ds5-device.cpp @@ -415,8 +415,7 @@ namespace librealsense for (auto& s : streams) { - depth_matchers.push_back(std::make_shared( s->get_unique_id(), s->get_stream_type(), - generate_matcher_name(s->get_stream_type(), s->get_unique_id()))); + depth_matchers.push_back(std::make_shared( s->get_unique_id(), s->get_stream_type())); } return std::make_shared(depth_matchers); } diff --git a/src/ds5/ds5-motion.cpp b/src/ds5/ds5-motion.cpp index 4e83116130..2a24c8027a 100644 --- a/src/ds5/ds5-motion.cpp +++ b/src/ds5/ds5-motion.cpp @@ -425,8 +425,7 @@ namespace librealsense std::vector> mm_matchers; for (auto& s : streams) - mm_matchers.push_back(std::make_shared( s->get_unique_id(), s->get_stream_type(), - generate_matcher_name(s->get_stream_type(), s->get_unique_id()))); + mm_matchers.push_back(std::make_shared( s->get_unique_id(), s->get_stream_type())); return std::make_shared(mm_matchers); } diff --git a/src/image.cpp b/src/image.cpp index b8c86fd707..fa33167290 100644 --- a/src/image.cpp +++ b/src/image.cpp @@ -782,7 +782,7 @@ namespace librealsense const native_pixel_format pf_y16 = { 'Y16 ', 1, 2,{ { true, &unpack_y16_from_y16_10, { { { RS2_STREAM_INFRARED, 1 }, RS2_FORMAT_Y16 } } } } }; const native_pixel_format pf_y8i = { 'Y8I ', 1, 2,{ { true, &unpack_y8_y8_from_y8i, { { { RS2_STREAM_INFRARED, 1 }, RS2_FORMAT_Y8 },{ { RS2_STREAM_INFRARED, 2 }, RS2_FORMAT_Y8 } } } } }; const native_pixel_format pf_y12i = { 'Y12I', 1, 3,{ { true, &unpack_y16_y16_from_y12i_10, { { { RS2_STREAM_INFRARED, 1 }, RS2_FORMAT_Y16 },{ { RS2_STREAM_INFRARED, 2 }, RS2_FORMAT_Y16 } } } } }; - const native_pixel_format pf_z16 = { 'Z16 ', 1, 2,{ { true, ©_pixels<2>, { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16 } } }, + const native_pixel_format pf_z16 = { 'Z16 ', 1, 2,{ { false, ©_pixels<2>, { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16 } } }, // The Disparity_Z is not applicable for D4XX. TODO - merge with INVZ when confirmed /*{ false, ©_pixels<2>, { { RS2_STREAM_DEPTH, RS2_FORMAT_DISPARITY16 } } }*/ } }; const native_pixel_format pf_invz = { 'Z16 ', 1, 2, { { false, ©_pixels<2>, { { RS2_STREAM_DEPTH, RS2_FORMAT_Z16 } } } } }; diff --git a/src/ivcam/sr300.cpp b/src/ivcam/sr300.cpp index 16becac45c..57fddf463a 100644 --- a/src/ivcam/sr300.cpp +++ b/src/ivcam/sr300.cpp @@ -314,14 +314,12 @@ namespace librealsense for (auto& s : streams) { - depth_matchers.push_back(std::make_shared( s->get_unique_id(), s->get_stream_type(), - generate_matcher_name(s->get_stream_type(),s->get_unique_id() ))); + depth_matchers.push_back(std::make_shared( s->get_unique_id(), s->get_stream_type())); } std::vector> matchers; matchers.push_back( std::make_shared(depth_matchers)); - auto color_matcher = std::make_shared( _color_stream->get_unique_id(), _color_stream->get_stream_type(), - generate_matcher_name(_color_stream->get_stream_type(), _color_stream->get_unique_id())); + auto color_matcher = std::make_shared( _color_stream->get_unique_id(), _color_stream->get_stream_type()); matchers.push_back(color_matcher); return std::make_shared(matchers); diff --git a/src/media/playback/playback_device.cpp b/src/media/playback/playback_device.cpp index 4a9a261ac9..8e8c2ce12d 100644 --- a/src/media/playback/playback_device.cpp +++ b/src/media/playback/playback_device.cpp @@ -189,8 +189,7 @@ std::shared_ptr playback_device::create_matcher(const frame_holder& fra //TOOD: Use future implementation of matcher factory with the device's name (or other unique identifier) LOG_WARNING("Playback device does not provide a matcher"); auto s = frame.frame->get_stream(); - return std::make_shared(s->get_unique_id(), s->get_stream_type(), - generate_matcher_name(s->get_stream_type(), s->get_unique_id())); + return std::make_shared(s->get_unique_id(), s->get_stream_type()); } void playback_device::set_frame_rate(double rate) diff --git a/src/sync.cpp b/src/sync.cpp index 5f10d20cec..dac0819bb8 100644 --- a/src/sync.cpp +++ b/src/sync.cpp @@ -22,7 +22,7 @@ namespace librealsense auto matched = composite->get_frame(i); ss << matched->get_stream()->get_stream_type() << " " << matched->get_frame_number() << ", "<get_frame_timestamp()<<"\n"; } - //std::cout< streams_id) + : _streams_id(streams_id){} + + void matcher::sync(frame_holder f, syncronization_environment env) { auto cb = begin_callback(); _callback(std::move(f), env); } + callback_invocation_holder matcher::begin_callback() + { + return{ _callback_inflight.allocate(), &_callback_inflight }; + } - identity_matcher::identity_matcher(stream_id stream, rs2_stream stream_type, std::string name) + matcher::~matcher() { - _name = name; - _streams_id = { stream }; - _stream_type = { stream_type }; + _callback_inflight.stop_allocation(); + + auto callbacks_inflight = _callback_inflight.get_size(); + if (callbacks_inflight > 0) + { + LOG_WARNING(callbacks_inflight << " callbacks are still running on some other threads. Waiting until all callbacks return..."); + } + // wait until user is done with all the stuff he chose to borrow + _callback_inflight.wait_until_empty(); } + identity_matcher::identity_matcher(stream_id stream, rs2_stream stream_type) + :matcher({stream}), + _name("I " + std::string(rs2_stream_to_string(stream_type))), + _stream_type {stream_type}{} + void identity_matcher::dispatch(frame_holder f, syncronization_environment env) { - //std::cout <<_name<<"--> "<< f->get_stream()->get_stream_type() << " " << f->get_frame_number() << ", "<get_frame_timestamp()<<"\n"; + std::stringstream s; + s <<_name<<"--> "<< f->get_stream()->get_stream_type() << " " << f->get_frame_number() << ", "<get_frame_timestamp()<<"\n"; + LOG_DEBUG(s.str()); + sync(std::move(f), env); } + std::string identity_matcher::get_name() const + { + return _name; + } + + const std::vector& identity_matcher::get_streams() const { return _streams_id; @@ -82,7 +109,7 @@ namespace librealsense composite_matcher::composite_matcher(std::vector> matchers, std::string name) { std::stringstream s; - s< "<< f->get_stream()->get_stream_type() << " " << f->get_frame_number() << ", "<get_frame_timestamp()<<"\n"; + std::stringstream s; + s <<"DISPATCH "<<_name<<"--> "<< f->get_stream()->get_stream_type() << " " << f->get_frame_number() << ", "<get_frame_timestamp()<<"\n"; + LOG_DEBUG(s.str()); - clean_dead_streams(f); + clean_inactive_streams(f); auto matcher = find_matcher(f); update_last_arrived(f, matcher.get()); matcher->dispatch(std::move(f), env); @@ -193,7 +228,7 @@ namespace librealsense { _frames_queue.erase(_matchers[stream_id].get()); } - _matchers[stream_id] = std::make_shared(stream_id, stream_type, generate_matcher_name(stream_type, stream_id)); + _matchers[stream_id] = std::make_shared(stream_id, stream_type); _streams.push_back(stream_id); _streams_type.push_back(stream_type); matcher = _matchers[stream_id]; @@ -211,7 +246,8 @@ namespace librealsense { std::stringstream s; s <<"SYNC "<<_name<<"--> "<< f->get_stream()->get_stream_type() << " " << f->get_frame_number() << ", "<get_frame_timestamp()<<"\n"; - // std::cout< match; match.reserve(synced_frames.size()); @@ -327,8 +363,7 @@ namespace librealsense } s<<"\n"; - //std::cout<allocate_composite_frame(std::move(match)); if (composite.frame) { @@ -369,19 +404,19 @@ namespace librealsense { return a->get_frame_number() < b->get_frame_number(); } - void frame_number_composite_matcher::clean_dead_streams(frame_holder& f) + void frame_number_composite_matcher::clean_inactive_streams(frame_holder& f) { - std::vector dead_matchers; + std::vector inactive_matchers; for(auto m: _matchers) { if(_last_arrived[m.second.get()] && (f->get_frame_number() - _last_arrived[m.second.get()]) > 5) { - dead_matchers.push_back(m.first); + inactive_matchers.push_back(m.first); m.second->set_active(false); } } - for(auto id: dead_matchers) + for(auto id: inactive_matchers) { _frames_queue[_matchers[id].get()].clear(); } @@ -466,7 +501,7 @@ namespace librealsense _next_expected_domain[matcher.get()] = f.frame->get_frame_timestamp_domain(); } - void timestamp_composite_matcher::clean_dead_streams(frame_holder& f) + void timestamp_composite_matcher::clean_inactive_streams(frame_holder& f) { std::vector dead_matchers; auto now = std::chrono::duration(std::chrono::system_clock::now().time_since_epoch()).count(); diff --git a/src/sync.h b/src/sync.h index 9544f82e9c..77b9c10a6b 100644 --- a/src/sync.h +++ b/src/sync.h @@ -3,9 +3,6 @@ #pragma once -#ifndef SYNC_H -#define SYNC_H - #include "types.h" #include "archive.h" @@ -19,7 +16,7 @@ namespace librealsense { typedef int stream_id; - static std::string generate_matcher_name(rs2_stream stream_type, stream_id stream) + inline std::string generate_matcher_name(rs2_stream stream_type, stream_id stream) { std::stringstream s; s<& matches; }; - - typedef std::function sync_callback; - class matcher + class matcher_interface { public: virtual void dispatch(frame_holder f, syncronization_environment env) = 0; - virtual void sync(frame_holder f, syncronization_environment env); - virtual void set_callback(sync_callback f); + virtual void sync(frame_holder f, syncronization_environment env) = 0; + virtual void set_callback(sync_callback f) = 0; virtual const std::vector& get_streams() const = 0; virtual const std::vector& get_streams_types() const = 0; + virtual std::string get_name() const = 0; + }; - callback_invocation_holder begin_callback() - { - return{ _callback_inflight.allocate(), &_callback_inflight }; - } - - virtual ~matcher() - { - _callback_inflight.stop_allocation(); + class matcher: public matcher_interface + { + public: + matcher(std::vector streams_id = {}); + virtual void sync(frame_holder f, syncronization_environment env); + virtual void set_callback(sync_callback f); - auto callbacks_inflight = _callback_inflight.get_size(); - if (callbacks_inflight > 0) - { - LOG_WARNING(callbacks_inflight << " callbacks are still running on some other threads. Waiting until all callbacks return..."); - } - // wait until user is done with all the stuff he chose to borrow - _callback_inflight.wait_until_empty(); - } - std::string get_name() - { - return _name; - } + callback_invocation_holder begin_callback(); + virtual ~matcher(); - bool get_active() const - { - return _active; - } + virtual std::string get_name() const {return "unknown matcher";} - void set_active(const bool active) - { - _active = active; - } + bool get_active() const {return _active;} + void set_active(const bool active){_active = active;} protected: - std::vector _streams_id; + std::vector _streams_id; + sync_callback _callback; + callbacks_heap _callback_inflight; - sync_callback _callback; - callbacks_heap _callback_inflight; - std::string _name; - - bool _active = true; + bool _active = true; }; class identity_matcher : public matcher { public: - identity_matcher(stream_id stream, rs2_stream streams_type, std::string name); + identity_matcher(stream_id stream, rs2_stream streams_type); void dispatch(frame_holder f, syncronization_environment env) override; const std::vector& get_streams() const override; const std::vector& get_streams_types() const override; + std::string get_name() const override; private: + std::string _name; std::vector _stream_type; }; @@ -163,7 +143,7 @@ namespace librealsense virtual bool are_equivalent(frame_holder& a, frame_holder& b) { return true; }; virtual bool is_smaller_than(frame_holder& a, frame_holder& b) { return true; }; virtual bool skip_missing_stream(std::vector synced, matcher* missing) { return false; }; - virtual void clean_dead_streams(frame_holder& f) = 0; + virtual void clean_inactive_streams(frame_holder& f) = 0; virtual void update_last_arrived(frame_holder& f, matcher* m) = 0; void dispatch(frame_holder f, syncronization_environment env) override; @@ -172,10 +152,12 @@ namespace librealsense const std::vector& get_streams_types() const override; std::shared_ptr find_matcher(const frame_holder& f); + std::string get_name() const override; + private: std::vector _streams; std::vector _streams_type; - + std::string _name; protected: virtual void update_next_expected(const frame_holder& f) = 0; @@ -194,7 +176,7 @@ namespace librealsense bool are_equivalent(frame_holder& a, frame_holder& b) override; bool is_smaller_than(frame_holder& a, frame_holder& b) override; bool skip_missing_stream(std::vector synced, matcher* missing) override; - void clean_dead_streams(frame_holder& f) override; + void clean_inactive_streams(frame_holder& f) override; void update_next_expected(const frame_holder& f) override; private: @@ -208,7 +190,7 @@ namespace librealsense bool are_equivalent(frame_holder& a, frame_holder& b) override; bool is_smaller_than(frame_holder& a, frame_holder& b) override; virtual void update_last_arrived(frame_holder& f, matcher* m) override; - void clean_dead_streams(frame_holder& f) override; + void clean_inactive_streams(frame_holder& f) override; bool skip_missing_stream(std::vector synced, matcher* missing) override; void update_next_expected(const frame_holder & f) override; @@ -233,4 +215,3 @@ namespace librealsense }; } -#endif diff --git a/tools/realsense-viewer/realsense-viewer.cpp b/tools/realsense-viewer/realsense-viewer.cpp index 6f8ea42ebc..0e2631bf94 100644 --- a/tools/realsense-viewer/realsense-viewer.cpp +++ b/tools/realsense-viewer/realsense-viewer.cpp @@ -143,7 +143,6 @@ void refresh_devices(std::mutex& m, dev_itr = current_connected_devices.erase(dev_itr); continue; - } ++dev_itr; } From 863d4e27517f244e8325b648e333f8f3a08adc87 Mon Sep 17 00:00:00 2001 From: aangerma Date: Thu, 9 Nov 2017 16:44:32 +0200 Subject: [PATCH 18/43] Disable non_syncronized_mode in depth-quality-tool. --- common/model-views.cpp | 37 ++++++++++++--------- common/model-views.h | 7 ++-- tools/depth-quality/depth-quality-model.cpp | 2 ++ 3 files changed, 27 insertions(+), 19 deletions(-) diff --git a/common/model-views.cpp b/common/model-views.cpp index 76f0cd852a..e88d52729b 100644 --- a/common/model-views.cpp +++ b/common/model-views.cpp @@ -1737,28 +1737,32 @@ namespace rs2 ImGui::SameLine(); - if (syncronize) + if(support_non_syncronized_mode) { - ImGui::PushStyleColor(ImGuiCol_Text, light_blue); - ImGui::PushStyleColor(ImGuiCol_TextSelectedBg, light_blue); - if (ImGui::Button(u8"\uf09c", { 24, top_bar_height })) + if (syncronize) { - syncronize = false; + ImGui::PushStyleColor(ImGuiCol_Text, light_blue); + ImGui::PushStyleColor(ImGuiCol_TextSelectedBg, light_blue); + if (ImGui::Button(u8"\uf09c", { 24, top_bar_height })) + { + syncronize = false; + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Disable syncronization between the pointcloud and the texture"); + ImGui::PopStyleColor(2); } - if (ImGui::IsItemHovered()) - ImGui::SetTooltip("Disable syncronization between the pointcloud and the texture"); - ImGui::PopStyleColor(2); - } - else - { - if (ImGui::Button(u8"\uf023", { 24, top_bar_height })) + else { - syncronize = true; + if (ImGui::Button(u8"\uf023", { 24, top_bar_height })) + { + syncronize = true; + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Keep the pointcloud and the texture sycronized"); } - if (ImGui::IsItemHovered()) - ImGui::SetTooltip("Keep the pointcloud and the texture sycronized"); } + ImGui::End(); ImGui::PopStyleColor(6); ImGui::PopStyleVar(); @@ -2659,7 +2663,7 @@ namespace rs2 { while (keep_calculating_pointcloud) { - if(streaming) + if(start_streaming) { try { @@ -3200,6 +3204,7 @@ namespace rs2 void viewer_model::upload_frame(frame&& f) { + pc.start(); auto index = f.get_profile().unique_id(); streams[index].upload_frame(std::move(f)); } diff --git a/common/model-views.h b/common/model-views.h index d1163eb7bb..12691b64f6 100644 --- a/common/model-views.h +++ b/common/model-views.h @@ -490,7 +490,7 @@ namespace rs2 }), viewer(viewer), keep_calculating_pointcloud(true), - streaming(false), + start_streaming(false), resulting_3d_models(1), t([this]() {render_loop(); }) { @@ -529,7 +529,7 @@ namespace rs2 } void start() { - streaming = true; + start_streaming = true; } private: @@ -542,7 +542,7 @@ namespace rs2 pointcloud pc; rs2::frameset model; std::atomic keep_calculating_pointcloud; - std::atomic streaming; + std::atomic start_streaming; frame_queue resulting_3d_models; @@ -627,6 +627,7 @@ namespace rs2 bool draw_plane = false; bool draw_frustrum = true; + bool support_non_syncronized_mode = true; bool syncronize = true; int selected_depth_source_uid = -1; diff --git a/tools/depth-quality/depth-quality-model.cpp b/tools/depth-quality/depth-quality-model.cpp index 9ae6bc9a80..4ea6ada668 100644 --- a/tools/depth-quality/depth-quality-model.cpp +++ b/tools/depth-quality/depth-quality-model.cpp @@ -23,6 +23,8 @@ namespace rs2 _viewer_model.allow_3d_source_change = false; _viewer_model.allow_stream_close = false; _viewer_model.draw_plane = true; + _viewer_model.syncronize = false; + _viewer_model.support_non_syncronized_mode = false; //pipeline outputs only syncronized frameset } bool tool_model::start(ux_window& window) From a802a31950a59209c7614cf1452d8a042adac1c1 Mon Sep 17 00:00:00 2001 From: "Shao,Ting" Date: Tue, 7 Nov 2017 17:07:25 +0800 Subject: [PATCH 19/43] [Node.js] Change FrameSet.at() to be based on FrameSet.getFrame() This could avoid creating a new Frame object each time at() is called, thus using less memory. --- wrappers/nodejs/index.js | 21 ++++----------------- wrappers/nodejs/src/addon.cpp | 35 ++++++++++++++++++++++++++--------- 2 files changed, 30 insertions(+), 26 deletions(-) diff --git a/wrappers/nodejs/index.js b/wrappers/nodejs/index.js index c0f0d74134..560d393ca2 100644 --- a/wrappers/nodejs/index.js +++ b/wrappers/nodejs/index.js @@ -1676,24 +1676,11 @@ class FrameSet { * @return {DepthFrame|VideoFrame|Frame|undefined} */ at(index) { - // - // TODO(ting): mapping index to stream and use this.cache[] - // e.g. return getFrame(this.cxxFrameSet.indexToStream(index)); - // - - let cxxFrame = this.cxxFrameSet.at(index); - - if (!cxxFrame) return undefined; - - if (cxxFrame.isDepthFrame()) { - return new DepthFrame(cxxFrame); + if ((arguments.length !== 1) || (typeof index !== 'number') || + (parseInt(index) >= this.size)) { + throw new TypeError('FrameSet.at(index) expects a valid integer argument'); } - - if (cxxFrame.isVideoFrame()) { - return new VideoFrame(cxxFrame); - } - - return new Frame(cxxFrame); + return this.getFrame(this.cxxFrameSet.indexToStream(index)); } __internalAssembleFrame(cxxFrame) { diff --git a/wrappers/nodejs/src/addon.cpp b/wrappers/nodejs/src/addon.cpp index ed23e4b2a3..99abee7ca1 100644 --- a/wrappers/nodejs/src/addon.cpp +++ b/wrappers/nodejs/src/addon.cpp @@ -1177,9 +1177,9 @@ class RSFrameSet : public Nan::ObjectWrap { Nan::SetPrototypeMethod(tpl, "destroy", Destroy); Nan::SetPrototypeMethod(tpl, "getSize", GetSize); - Nan::SetPrototypeMethod(tpl, "at", At); Nan::SetPrototypeMethod(tpl, "getFrame", GetFrame); Nan::SetPrototypeMethod(tpl, "replaceFrame", ReplaceFrame); + Nan::SetPrototypeMethod(tpl, "indexToStream", IndexToStream); constructor_.Reset(tpl->GetFunction()); exports->Set(Nan::New("RSFrameSet").ToLocalChecked(), tpl->GetFunction()); @@ -1306,17 +1306,34 @@ class RSFrameSet : public Nan::ObjectWrap { info.GetReturnValue().Set(Nan::False()); } - static NAN_METHOD(At) { + static NAN_METHOD(IndexToStream) { auto me = Nan::ObjectWrap::Unwrap(info.Holder()); int32_t index = info[0]->IntegerValue(); - if (me && me->frames_) { - rs2_frame* frame = rs2_extract_frame(me->frames_, index, &me->error_); - if (frame) { - info.GetReturnValue().Set(RSFrame::NewInstance(frame)); - return; - } + if (!(me && me->frames_)) { + info.GetReturnValue().Set(Nan::Undefined()); + return; } - info.GetReturnValue().Set(Nan::Undefined()); + rs2_frame* frame = rs2_extract_frame(me->frames_, index, &me->error_); + if (!frame) { + info.GetReturnValue().Set(Nan::Undefined()); + return; + } + const rs2_stream_profile* profile = rs2_get_frame_stream_profile( + frame, &me->error_); + if (!profile) { + rs2_release_frame(frame); + info.GetReturnValue().Set(Nan::Undefined()); + return; + } + rs2_stream stream = RS2_STREAM_ANY; + rs2_format format = RS2_FORMAT_ANY; + int32_t fps = 0; + int32_t idx = 0; + int32_t unique_id = 0; + rs2_get_stream_profile_data(profile, &stream, &format, &idx, + &unique_id, &fps, &me->error_); + info.GetReturnValue().Set(Nan::New(stream)); + rs2_release_frame(frame); } private: From e77819ab0560985845bd7b4238b93a1febdccc08 Mon Sep 17 00:00:00 2001 From: icarpis Date: Sun, 12 Nov 2017 18:03:41 +0200 Subject: [PATCH 20/43] Fixed dropped frame issue of color streaming Change-Id: I4f89fcb5efc8fe069162b22ab1e3174e74503c6e --- src/backend.h | 10 +++++----- src/libuvc/libuvc.cpp | 2 +- src/linux/backend-v4l2.cpp | 7 ++----- src/linux/backend-v4l2.h | 2 +- src/mock/recorder.cpp | 8 ++++---- src/mock/recorder.h | 4 ++-- src/sensor.cpp | 4 ++-- src/source.cpp | 2 +- src/win/win-uvc.cpp | 2 +- src/win/win-uvc.h | 2 +- wrappers/python/pybackend.cpp | 2 +- 11 files changed, 21 insertions(+), 24 deletions(-) diff --git a/src/backend.h b/src/backend.h index 0d8a0ecbb9..f70df395c1 100644 --- a/src/backend.h +++ b/src/backend.h @@ -362,7 +362,7 @@ namespace librealsense class uvc_device { public: - virtual void probe_and_commit( stream_profile profile, bool zero_copy, frame_callback callback, int buffers = DEFAULT_FRAME_BUFFERS) = 0; + virtual void probe_and_commit(stream_profile profile, frame_callback callback, int buffers = DEFAULT_FRAME_BUFFERS) = 0; virtual void stream_on(std::function error_handler = [](const notification& n){}) = 0; virtual void start_callbacks() = 0; virtual void stop_callbacks() = 0; @@ -400,9 +400,9 @@ namespace librealsense explicit retry_controls_work_around(std::shared_ptr dev) : _dev(dev) {} - void probe_and_commit( stream_profile profile, bool zero_copy, frame_callback callback, int buffers) override + void probe_and_commit(stream_profile profile, frame_callback callback, int buffers) override { - _dev->probe_and_commit(profile, zero_copy, callback, buffers); + _dev->probe_and_commit(profile, callback, buffers); } void stream_on(std::function error_handler = [](const notification& n){}) override @@ -661,11 +661,11 @@ namespace librealsense : _dev(dev) {} - void probe_and_commit( stream_profile profile, bool zero_copy, frame_callback callback, int buffers) override + void probe_and_commit(stream_profile profile, frame_callback callback, int buffers) override { auto dev_index = get_dev_index_by_profiles(profile); _configured_indexes.insert(dev_index); - _dev[dev_index]->probe_and_commit(profile, zero_copy, callback, buffers); + _dev[dev_index]->probe_and_commit(profile, callback, buffers); } diff --git a/src/libuvc/libuvc.cpp b/src/libuvc/libuvc.cpp index 0f240fdf96..20698c4e16 100644 --- a/src/libuvc/libuvc.cpp +++ b/src/libuvc/libuvc.cpp @@ -294,7 +294,7 @@ namespace librealsense } /* request to set up a streaming profile and its calback */ - void probe_and_commit(stream_profile profile, bool zero_copy, frame_callback callback, int buffers) override + void probe_and_commit(stream_profile profile, frame_callback callback, int buffers) override { uvc_error_t res; uvc_stream_ctrl_t ctrl; diff --git a/src/linux/backend-v4l2.cpp b/src/linux/backend-v4l2.cpp index 387f5c6416..c80a080688 100644 --- a/src/linux/backend-v4l2.cpp +++ b/src/linux/backend-v4l2.cpp @@ -517,11 +517,8 @@ namespace librealsense if (_thread) _thread->join(); } - void v4l_uvc_device::probe_and_commit( stream_profile profile, bool zero_copy, frame_callback callback, int buffers) + void v4l_uvc_device::probe_and_commit(stream_profile profile, frame_callback callback, int buffers) { - if(!zero_copy) - buffers = 1; - if(!_is_capturing && !_callback) { v4l2_fmtdesc pixel_format = {}; @@ -538,7 +535,7 @@ namespace librealsense // Microsoft Depth GUIDs for R400 series are not yet recognized // by the Linux kernel, but they do not require a patch, since there // are "backup" Z16 and Y8 formats in place - std::set known_problematic_formats = { + static const std::set known_problematic_formats = { "00000050-0000-0010-8000-00aa003", "00000032-0000-0010-8000-00aa003", }; diff --git a/src/linux/backend-v4l2.h b/src/linux/backend-v4l2.h index db6e03d431..d5406334ea 100644 --- a/src/linux/backend-v4l2.h +++ b/src/linux/backend-v4l2.h @@ -141,7 +141,7 @@ namespace librealsense ~v4l_uvc_device(); - void probe_and_commit( stream_profile profile, bool zero_copy, frame_callback callback, int buffers) override; + void probe_and_commit(stream_profile profile, frame_callback callback, int buffers) override; void stream_on(std::function error_handler) override; diff --git a/src/mock/recorder.cpp b/src/mock/recorder.cpp index 22ca60f117..40ef7fc41d 100644 --- a/src/mock/recorder.cpp +++ b/src/mock/recorder.cpp @@ -628,11 +628,11 @@ namespace librealsense } - void record_uvc_device::probe_and_commit( stream_profile profile, bool zero_copy, frame_callback callback, int buffers) + void record_uvc_device::probe_and_commit(stream_profile profile, frame_callback callback, int buffers) { - _owner->try_record([this, zero_copy, callback, profile](recording* rec, lookup_key k) + _owner->try_record([this, callback, profile](recording* rec, lookup_key k) { - _source->probe_and_commit(profile, zero_copy, [this, callback](stream_profile p, frame_object f, std::function continuation) + _source->probe_and_commit(profile, [this, callback](stream_profile p, frame_object f, std::function continuation) { _owner->try_record([this, callback, p, &f, continuation](recording* rec1, lookup_key key1) { @@ -1179,7 +1179,7 @@ namespace librealsense }); } - void playback_uvc_device::probe_and_commit( stream_profile profile, bool zero_copy, frame_callback callback, int buffers) + void playback_uvc_device::probe_and_commit(stream_profile profile, frame_callback callback, int buffers) { auto stored = _rec->load_stream_profiles(_entity_id, call_type::uvc_probe_commit); vector input{ profile }; diff --git a/src/mock/recorder.h b/src/mock/recorder.h index bcf8086fd8..62d6db99c8 100644 --- a/src/mock/recorder.h +++ b/src/mock/recorder.h @@ -324,7 +324,7 @@ namespace librealsense class record_uvc_device : public uvc_device { public: - void probe_and_commit( stream_profile profile, bool zero_copy, frame_callback callback, int buffers) override; + void probe_and_commit(stream_profile profile, frame_callback callback, int buffers) override; void stream_on(std::function error_handler = [](const notification& n) {}) override; void start_callbacks() override; void stop_callbacks() override; @@ -502,7 +502,7 @@ namespace librealsense class playback_uvc_device : public uvc_device { public: - void probe_and_commit( stream_profile profile, bool zero_copy, frame_callback callback, int buffers) override; + void probe_and_commit(stream_profile profile, frame_callback callback, int buffers) override; void stream_on(std::function error_handler = [](const notification& n) {}) override; void start_callbacks() override; void stop_callbacks() override; diff --git a/src/sensor.cpp b/src/sensor.cpp index 2e1f791502..bf1018022b 100644 --- a/src/sensor.cpp +++ b/src/sensor.cpp @@ -318,7 +318,7 @@ namespace librealsense { try { - _device->probe_and_commit(mode.profile, !mode.requires_processing(), + _device->probe_and_commit(mode.profile, [this, mode, timestamp_reader, requests](platform::stream_profile p, platform::frame_object f, std::function continuation) mutable { auto system_time = environment::get_instance().get_time_service()->get_time(); @@ -420,7 +420,7 @@ namespace librealsense _source.invoke_callback(std::move(pref)); } }, - static_cast(_source.get_published_size_option()->query())); + DEFAULT_FRAME_BUFFERS); } catch(...) { diff --git a/src/source.cpp b/src/source.cpp index 0ebb44b67b..1db33402e5 100644 --- a/src/source.cpp +++ b/src/source.cpp @@ -38,7 +38,7 @@ namespace librealsense std::shared_ptr