Added unpacker for l500 confidence streaming

zokin · Mar 29, 2018 · d5989a0 · d5989a0
1 parent 0a61333
commit d5989a0
Show file tree

Hide file tree

Showing 10 changed files with 118 additions and 78 deletions.
diff --git a/include/librealsense2/h/rs_sensor.h b/include/librealsense2/h/rs_sensor.h
@@ -44,7 +44,7 @@ typedef enum rs2_stream
     RS2_STREAM_ACCEL                            , /**< Native stream of accelerometer motion data produced by RealSense device */
     RS2_STREAM_GPIO                             , /**< Signals from external device connected through GPIO */
     RS2_STREAM_POSE                             , /**< 6 Degrees of Freedom pose data, calculated by RealSense device */ 
-    RS2_STREAM_CONFIDENCE_MAP,
+    RS2_STREAM_CONFIDENCE,
     RS2_STREAM_COUNT
 } rs2_stream;
 const char* rs2_stream_to_string(rs2_stream stream);

diff --git a/src/image.cpp b/src/image.cpp
diff --git a/src/l500/l500.cpp b/src/l500/l500.cpp
@@ -64,7 +64,7 @@ namespace librealsense
         _depth_device_idx(add_sensor(create_depth_device(ctx, group.uvc_devices))),
         _depth_stream(new stream(RS2_STREAM_DEPTH)),
         _ir_stream(new stream(RS2_STREAM_INFRARED)),
-        _confidence_stream(new stream(RS2_STREAM_CONFIDENCE_MAP))
+        _confidence_stream(new stream(RS2_STREAM_CONFIDENCE))
     {
         static const auto device_name = "Intel RealSense L500";
 

diff --git a/src/l500/l500.h b/src/l500/l500.h
@@ -131,7 +131,7 @@ namespace librealsense
                     {
                         assign_stream(_owner->_ir_stream, p);
                     }
-                    else if (p->get_stream_type() == RS2_STREAM_CONFIDENCE_MAP)
+                    else if (p->get_stream_type() == RS2_STREAM_CONFIDENCE)
                     {
                         assign_stream(_owner->_confidence_stream, p);
                     }
@@ -144,6 +144,9 @@ namespace librealsense
                     if (video->get_width() == 640 && video->get_height() == 480 && video->get_format() == RS2_FORMAT_Y8 && video->get_framerate() == 30)
                         video->make_default();
 
+                    if (video->get_width() == 640 && video->get_height() == 480 && video->get_format() == RS2_FORMAT_RAW8 && video->get_framerate() == 30)
+                        video->make_default();
+
                     auto profile = to_profile(p.get());
                     std::weak_ptr<l500_depth_sensor> wp =
                         std::dynamic_pointer_cast<l500_depth_sensor>(this->shared_from_this());

diff --git a/src/media/ros/ros_file_format.h b/src/media/ros/ros_file_format.h
@@ -401,6 +401,7 @@ namespace librealsense
         {
             switch (type)
             {
+            case RS2_STREAM_CONFIDENCE:
             case RS2_STREAM_DEPTH:
             case RS2_STREAM_COLOR:
             case RS2_STREAM_INFRARED:

diff --git a/src/sensor.cpp b/src/sensor.cpp
@@ -259,7 +259,7 @@ namespace librealsense
         std::set<uint32_t> registered_formats;
 
         power on(std::dynamic_pointer_cast<uvc_sensor>(shared_from_this()));
-        if (_uvc_profiles.empty())
+        if (_uvc_profiles.empty()){}
             _uvc_profiles = _device->get_profiles();
 
         for (auto&& p : _uvc_profiles)
@@ -447,12 +447,15 @@ namespace librealsense
                         last_frame_number = frame_counter;
                         last_timestamp = timestamp;
 
-                        frame_holder frame = _source.alloc_frame(stream_to_frame_types(output.stream_desc.type), mode.profile.width * mode.profile.height * bpp / 8, additional_data, requires_processing);
+                        auto res = output.stream_resolution({ mode.profile.width, mode.profile.height });
+                        auto width = res.width;
+                        auto height = res.height;
+
+                        frame_holder frame = _source.alloc_frame(stream_to_frame_types(output.stream_desc.type), width * height * bpp / 8, additional_data, requires_processing);
                         if (frame.frame)
                         {
-                            auto res = output.stream_resolution({ mode.profile.width, mode.profile.height });
                             auto video = (video_frame*)frame.frame;
-                            video->assign(res.width, res.height, res.width * bpp / 8, bpp);
+                            video->assign(width, height, width * bpp / 8, bpp);
                             video->set_timestamp_domain(timestamp_domain);
                             dest.push_back(const_cast<byte*>(video->get_frame_data()));
                             frame->set_stream(request);

diff --git a/src/types.cpp b/src/types.cpp
@@ -121,7 +121,7 @@ namespace librealsense
                 STRCASE(STREAM, ACCEL)
                 STRCASE(STREAM, GPIO)
                 STRCASE(STREAM, POSE)
-                STRCASE(STREAM, CONFIDENCE_MAP)
+                STRCASE(STREAM, CONFIDENCE)
         default: assert(!is_valid(value)); return UNKNOWN_VALUE;
         }
 #undef CASE

diff --git a/src/types.h b/src/types.h
@@ -530,9 +530,9 @@ namespace librealsense
             {
                 for (auto & o : outputs)
                 {
-                    auto res = o.stream_resolution(resolution{ request.width, request.height });
+                    auto res = o.stream_resolution(resolution{ uvc_p.width, uvc_p.height });
                     if (o.stream_desc.type == request.stream && o.stream_desc.index == request.index &&
-                        res.width == uvc_p.width && res.height == uvc_p.height &&
+                        res.width == request.width && res.height == request.height &&
                         uvc_p.format == fourcc && request.fps == uvc_p.fps)
                         return true;
                 }
@@ -556,9 +556,9 @@ namespace librealsense
         {
             for (auto& o : outputs)
             {
-                auto res = o.stream_resolution(resolution{ request.width, request.height });
+                auto res = o.stream_resolution(resolution{ uvc_profile.width, uvc_profile.height });
                 if (o.stream_desc.type == request.stream && o.stream_desc.index == request.index &&
-                    res.width == uvc_profile.width && res.height == uvc_profile.height)
+                    res.width == request.width && res.height == request.height)
                     return true;
             }
 

diff --git a/wrappers/csharp/Intel.RealSense/Types.cs b/wrappers/csharp/Intel.RealSense/Types.cs
@@ -88,6 +88,8 @@ public enum Stream
         Gyro = 5,
         Accel = 6,
         Gpio = 7,
+        Pose = 8,
+        Confidence = 9,
     }
 
     public enum Format

diff --git a/wrappers/nodejs/src/addon.cpp b/wrappers/nodejs/src/addon.cpp
@@ -4469,6 +4469,7 @@ void InitModule(v8::Local<v8::Object> exports) {
   _FORCE_SET_ENUM(RS2_STREAM_ACCEL);
   _FORCE_SET_ENUM(RS2_STREAM_GPIO);
   _FORCE_SET_ENUM(RS2_STREAM_COUNT);
+  // TODO(icarpis) Add RS2_STREAM_POSE and RS2_STREAM_CONFIDENCE
 
   // rs2_format
   _FORCE_SET_ENUM(RS2_FORMAT_ANY);