Adding depth units value to frame metadata #9154

nohayassin · 2021-06-03T15:29:23Z

Track on DSO-16066

ev-mp · 2021-06-06T00:03:23Z

src/sensor.cpp

+    void uvc_sensor::set_depth_units(float value)
+    {
+        _depth_units = value;
+    }


Applies to Depth sensor only. Don't put in the base class
Add comment - TODO for refactoring

src/ds5/ds5-device.cpp

ev-mp · 2021-06-06T00:05:32Z

src/proc/colorizer.cpp

-                }
-            }
+            depth_units = ((depth_frame*)f.get())->get_units();
+


Revert the playback sensor

@ev-mp I still think this part is redundant because depth_units is defined the same way regardless to playback condition, but I will add it for you to see

ev-mp · 2021-06-06T00:09:17Z

src/proc/colorizer.cpp

@@ -289,7 +271,7 @@ namespace librealsense
            }
        };

-        auto make_value_cropped_frame = [this](const rs2::video_frame& depth, rs2::video_frame rgb)
+        auto make_value_cropped_frame = [this, depth_units](const rs2::video_frame& depth, rs2::video_frame rgb)


Retrieve the depth units from the depth frame

ev-mp · 2021-06-06T09:48:50Z

src/metadata.h

@@ -375,6 +375,7 @@ namespace librealsense
    struct md_depth_control
    {
        md_header   header;
+        uint32_t    depth_units;


The metadata is part of FW API - remove it

ev-mp · 2021-06-06T10:09:24Z

src/proc/colorizer.cpp

-                    throw std::runtime_error( "failed to query depth units from sensor" );
-                }
-            }
+            depth_units = ((depth_frame*)f.get())->get_units();


Should be checked externally

ev-mp · 2021-06-06T10:11:58Z

src/proc/pointcloud.cpp

@@ -253,8 +245,8 @@ namespace librealsense
    {
        auto res = allocate_points(source, depth);
        auto pframe = (librealsense::points*)(res.get());
-
-        const float3* points = depth_to_points(res, *_depth_intrinsics, depth, *_depth_units);
+        auto depth_units = ((depth_frame*)depth.get())->get_units();


Check if the member should be updated instead of local

ev-mp · 2021-06-06T10:16:14Z

src/sensor.h

@@ -98,7 +98,8 @@ namespace librealsense
            frame_timestamp_reader* timestamp_reader,
            const rs2_time_t& last_timestamp,
            const unsigned long long& last_frame_number,
-            std::shared_ptr<stream_profile_interface> profile);
+            std::shared_ptr<stream_profile_interface> profile,
+            float depth_units = -1);


don't use default values

ev-mp · 2021-06-07T13:38:44Z

src/media/ros/ros_writer.cpp

@@ -190,6 +190,7 @@ namespace librealsense
        image.header.stamp = rs2rosinternal::Time(std::chrono::duration<double>(timestamp_ms).count());
        std::string TODO_CORRECT_ME = "0";
        image.header.frame_id = TODO_CORRECT_ME;
+        image.header.depth_units = static_cast<float>(vid_frame->get_frame_depth_units());


Need to be retrieved using librealsense:: methods only

ev-mp · 2021-06-07T13:40:44Z

include/librealsense2/hpp/rs_frame.hpp

+        float get_frame_depth_units() const
+        {
+            rs2_error* e = nullptr;
+            auto r = rs2_depth_frame_get_units(frame_ref, &e);
+            error::handle(e);
+            return r;
+        }
+


ev-mp · 2021-06-07T13:41:23Z

src/archive.h

@@ -41,6 +41,7 @@ namespace librealsense
        bool                is_blocking = false; // when running from recording, this bit indicates 
                                                 // if the recorder was configured to realtime mode or not
                                                 // if true, this will force any queue receiving this frame not to drop it
+        float               depth_units = 0.0f;


Add comment that this is a temporal solution

ev-mp · 2021-06-07T13:42:56Z

src/archive.h

+        float get_frame_depth_units() const override
+        {
+            return first()->get_frame_depth_units();
+        }


ev-mp · 2021-06-07T13:43:01Z

src/archive.h

@@ -134,6 +137,7 @@ namespace librealsense
        rs2_timestamp_domain get_frame_timestamp_domain() const override;
        void set_timestamp(double new_ts) override { additional_data.timestamp = new_ts; }
        unsigned long long get_frame_number() const override;
+        float get_frame_depth_units() const override;


ev-mp · 2021-06-07T13:47:21Z

src/gl/colorizer-gl.cpp

@@ -202,7 +202,7 @@ namespace librealsense
                _width = vp.width(); _height = vp.height();

                auto info = disparity_info::update_info_from_frame(f);
-                _depth_units = info.depth_units;
+                _depth_units = ((depth_frame*)f.get())->get_units();


Must be performed each iteration - move outside of the profile update clause

ev-mp · 2021-06-07T13:49:50Z

src/proc/colorizer.cpp

@@ -235,32 +235,13 @@ namespace librealsense

    rs2::frame colorizer::process_frame(const rs2::frame_source& source, const rs2::frame& f)
    {
+        _depth_units = ((depth_frame*)f.get())->get_units();


Check frame type before dereferencing

ev-mp · 2021-06-07T13:52:07Z

src/proc/disparity-transform.h

@@ -82,6 +82,7 @@ namespace librealsense
            librealsense::depth_stereo_sensor* dss;
            auto info = disparity_info::info();
            float stereo_baseline_meter;
+            info.depth_units = ((depth_frame*)f.get())->get_units();


Verify frame's type is depth

ev-mp · 2021-06-07T13:55:55Z

src/proc/synthetic-stream.cpp

@@ -331,6 +331,7 @@ namespace librealsense
            data.metadata_size = 0;
            data.system_time = _actual_source.get_time();
            data.is_blocking = original->is_blocking();
+            data.depth_units = original->get_frame_depth_units();


Please check if required

ev-mp · 2021-06-07T13:58:40Z

src/tm2/tm-device.cpp

@@ -1092,7 +1092,7 @@ namespace librealsense
        pose_frame_metadata frame_md = { 0 };
        frame_md.arrival_ts = duration_cast<std::chrono::nanoseconds>(ts.arrival_ts).count();

-        frame_additional_data additional_data(ts.device_ts.count(), frame_num++, ts.arrival_ts.count(), sizeof(frame_md), (uint8_t*)&frame_md, ts.global_ts.count(), 0, 0, false);
+        frame_additional_data additional_data(ts.device_ts.count(), frame_num++, ts.arrival_ts.count(), sizeof(frame_md), (uint8_t*)&frame_md, ts.global_ts.count(), 0, 0, false, 0.0);


Check if can be dropped - no need to explicitly set for T265

it should stay, the API with default depth_units=-1 is not related to this

ev-mp

The actual dependency injection is not clear yet.
For review

ev-mp · 2021-06-14T16:33:45Z

src/proc/synthetic-stream.cpp

+                    // For old playback sensors
+                    if (!((depth_frame*)f.get())->get_units())
+                    {
+                        auto snr = ((frame_interface*)f.get())->get_sensor().get();
+                        auto depth_sensor = As< librealsense::depth_sensor >(snr);
+                        auto extendable = As< librealsense::extendable_interface >(snr);
+                        if (extendable && extendable->extend_to(TypeToExtension< librealsense::depth_sensor >::value, (void**)(&depth_sensor)))
+                        {
+                            auto du = depth_sensor->get_depth_scale();
+                            ((depth_frame*)f.get())->set_units(du);
+                        }
+                    }


The DU injection should occur during the frame construction in uvc_sensor::open( or playback

ev-mp · 2021-06-14T18:15:02Z

src/sensor.cpp

-        // generate additional data
+        float depth_units = 0;
+        if(_on_frame)
+            _on_frame(depth_units);//_additional_data.depth_units;


The callback should get the additional_data handle and modify its content

ev-mp · 2021-06-14T18:15:56Z

src/sensor.h

@@ -30,6 +30,7 @@ namespace librealsense
    class option;

    typedef std::function<void(std::vector<platform::stream_profile>)> on_open;
+    typedef std::function<void(float &val)> on_frame_md;


The function should receive a reference to additional_data and modify its content

ev-mp · 2021-06-14T18:19:57Z

src/sensor.h


        std::vector<platform::stream_profile> _internal_config;

        std::atomic<bool> _is_streaming;
        std::atomic<bool> _is_opened;
        std::shared_ptr<notifications_processor> _notifications_processor;
        on_open _on_open;
+        on_frame_md _on_frame;


Rename to metadata_modifier for clarity

ev-mp · 2021-06-14T18:20:32Z

src/sensor.h

@@ -353,6 +354,10 @@ namespace librealsense
            return action(*_device);
        }

+        void update_params(on_frame_md callback) override


Probably should be called set_metadata_modifier or similar

ev-mp · 2021-06-14T18:23:43Z

src/archive.h

+        void set_units(float depth_units)
+        {
+            additional_data.depth_units = depth_units;
+        }
+


Shouldn't be needed

ev-mp · 2021-06-14T18:29:21Z

src/media/ros/ros_writer.cpp

@@ -190,7 +190,7 @@ namespace librealsense
        image.header.stamp = rs2rosinternal::Time(std::chrono::duration<double>(timestamp_ms).count());
        std::string TODO_CORRECT_ME = "0";
        image.header.frame_id = TODO_CORRECT_ME;
-        image.header.depth_units = static_cast<float>(((depth_frame*)vid_frame)->get_units());
+        image.depth_units = static_cast<float>(((depth_frame*)vid_frame)->get_units());


Let's review the record/playback mechanims

ev-mp · 2021-06-14T18:31:54Z

third-party/realsense-file/rosbag/msgs/sensor_msgs/Image.h

@@ -238,6 +242,14 @@ namespace serialization
      stream.next(m.is_bigendian);
      stream.next(m.step);
      stream.next(m.data);
+      try
+      {
+          stream.next(m.depth_units);


There is a header that should be used to differentiate according to version number

ev-mp · 2021-06-14T18:33:26Z

tools/rosbag-inspector/print_helpers.h

            os << "Encoding     : " << image->encoding << std::endl;
            os << "Width        : " << image->width << std::endl;
            os << "Height       : " << image->height << std::endl;
            os << "Step         : " << image->step << std::endl;
+            //os << "Frame Number : " << image->header.seq << std::endl;


remove the comment

ev-mp · 2021-06-16T08:06:49Z

src/ds5/ds5-device.cpp

+        {
+            _metadata_modifier = callback;
+            auto s = get_raw_sensor().get();
+            As< librealsense::uvc_sensor >(s)->modify_frame_metadata(callback);


Rename to set_frame_metadata_modifier

ev-mp · 2021-06-16T08:15:59Z

src/ds5/ds5-device.cpp

+        void set_depth_scale(float val)
+        {
+            _depth_units = val;
+            modify_frame_metadata([&](frame_additional_data& data) {data.depth_units = _depth_units.load(); });


Check _depth_units lifetime

ev-mp · 2021-06-16T08:18:55Z

src/gl/colorizer-gl.cpp

@@ -202,7 +204,6 @@ namespace librealsense
                _width = vp.width(); _height = vp.height();

                auto info = disparity_info::update_info_from_frame(f);
-                _depth_units = info.depth_units;


Remove the field depth_units from disparity_info

ev-mp · 2021-06-16T08:32:41Z

src/media/ros/ros_writer.cpp

-        std::string TODO_CORRECT_ME = "0";
-        image.header.frame_id = TODO_CORRECT_ME;
+        std::string NEW_ROSBAG = "1";
+        image.header.frame_id = NEW_ROSBAG;


Rename to version + comment for maintainers

ev-mp · 2021-06-16T08:33:03Z

src/media/ros/ros_writer.cpp

@@ -188,8 +188,9 @@ namespace librealsense
        image.header.seq = static_cast<uint32_t>(vid_frame->get_frame_number());
        std::chrono::duration<double, std::milli> timestamp_ms(vid_frame->get_frame_timestamp());
        image.header.stamp = rs2rosinternal::Time(std::chrono::duration<double>(timestamp_ms).count());
-        std::string TODO_CORRECT_ME = "0";
-        image.header.frame_id = TODO_CORRECT_ME;
+        std::string NEW_ROSBAG = "1";


No need for it

ev-mp · 2021-06-16T08:34:28Z

src/media/ros/ros_writer.cpp

-        image.header.frame_id = TODO_CORRECT_ME;
+        std::string NEW_ROSBAG = "1";
+        image.header.frame_id = NEW_ROSBAG;
+        image.depth_units = static_cast<float>(((depth_frame*)vid_frame)->get_units());


Must be dynamically checked

ev-mp · 2021-06-16T08:35:48Z

third-party/realsense-file/rosbag/msgs/sensor_msgs/Image.h

+    s << indent << "depth_units: ";
+    Printer<float>::stream(s, indent + "  ", v.depth_units);


Print only for depth frame

ev-mp · 2021-06-20T14:40:34Z

third-party/realsense-file/rosbag/msgs/sensor_msgs/Image.h

@@ -281,8 +281,11 @@ struct Printer< ::sensor_msgs::Image_<ContainerAllocator> >
      s << indent << "  data[" << i << "]: ";
      Printer<uint8_t>::stream(s, indent + "  ", v.data[i]);
    }
-    s << indent << "depth_units: ";
-    Printer<float>::stream(s, indent + "  ", v.depth_units);
+    if (v.depth_units)


Check for floating_point != zero explicitly using numeric_limits

ev-mp · 2021-06-20T14:42:40Z

third-party/realsense-file/rosbag/msgs/std_msgs/Header.h

@@ -44,7 +44,7 @@ struct Header_
  _stamp_type stamp;

   typedef std::basic_string<char, std::char_traits<char>, typename ContainerAllocator::template rebind<char>::other >  _frame_id_type;
-  _frame_id_type frame_id;
+  _frame_id_type version;


Provide comment for maintainers

ev-mp · 2021-06-20T14:45:12Z

src/ds5/ds5-device.cpp

        {
            _metadata_modifier = callback;
            auto s = get_raw_sensor().get();
-            As< librealsense::uvc_sensor >(s)->modify_frame_metadata(callback);
+            As< librealsense::uvc_sensor >(s)->set_frame_metadata_modifier(callback);


Check that the extension conversion is not null before dereferencing

ev-mp · 2021-06-20T14:46:24Z

src/media/ros/ros_writer.cpp

@@ -207,8 +208,7 @@ namespace librealsense
        imu_msg.header.seq = static_cast<uint32_t>(frame.frame->get_frame_number());
        std::chrono::duration<double, std::milli> timestamp_ms(frame.frame->get_frame_timestamp());
        imu_msg.header.stamp = rs2rosinternal::Time(std::chrono::duration<double>(timestamp_ms).count());
-        std::string TODO_CORRECT_ME = "0";
-        imu_msg.header.frame_id = TODO_CORRECT_ME;
+        imu_msg.header.version = "1"; // used to distinguish between old rosbag and new rosbag that contains depth units in frame metadata


Add comment that the field is unused and therefore assigned for ROSbag versions control

ev-mp

Fix the last comment

ev-mp · 2021-06-21T08:22:41Z

third-party/realsense-file/rosbag/msgs/sensor_msgs/Image.h

@@ -281,7 +281,7 @@ struct Printer< ::sensor_msgs::Image_<ContainerAllocator> >
      s << indent << "  data[" << i << "]: ";
      Printer<uint8_t>::stream(s, indent + "  ", v.data[i]);
    }
-    if (v.depth_units)
+    if (v.depth_units != 0)


Use std::numeric_limits<float>::min() as in original comment

ev-mp requested changes Jun 6, 2021

View reviewed changes

ev-mp reviewed Jun 6, 2021

View reviewed changes

nohayassin force-pushed the dso_16066_04 branch from 9fc8562 to 3e6ea1d Compare June 6, 2021 19:40

nohayassin added 2 commits June 7, 2021 14:21

added frame metadata

80fbbee

added depth units to md

947f067

nohayassin force-pushed the dso_16066_04 branch from 303162f to c73dcd6 Compare June 7, 2021 11:22

added depth units to md

a1d0aa1

nohayassin force-pushed the dso_16066_04 branch from c73dcd6 to a1d0aa1 Compare June 7, 2021 11:42

ev-mp requested changes Jun 7, 2021

View reviewed changes

nohayassin added 6 commits June 7, 2021 18:04

fixes

57bc1e5

new design

f298178

fixed design

5d320d9

fixed design

f9f314c

fixed playback for old rosbag

cdee6b0

fixes

1217402

ev-mp requested changes Jun 14, 2021

View reviewed changes

nohayassin added 4 commits June 15, 2021 11:03

fixes

50c1099

DU injection debug

ad74523

fixed DU injection

5609b49

fixed DU injection

b693574

ev-mp requested changes Jun 16, 2021

View reviewed changes

fixes

fde3cd6

ev-mp requested changes Jun 20, 2021

View reviewed changes

fixes

0065af3

ev-mp requested changes Jun 21, 2021

View reviewed changes

fix

208b390

ev-mp approved these changes Jun 21, 2021

View reviewed changes

ev-mp merged commit 161cee2 into IntelRealSense:development Jun 21, 2021

ev-mp mentioned this pull request Jun 23, 2021

Rosbag-Inspector fix #9274

Merged

ev-mp mentioned this pull request Jul 24, 2021

CUDA build fix #9482

Merged

maloel mentioned this pull request Jul 26, 2021

Fix pointcloud for L515 #9490

Merged

		s << indent << "depth_units: ";
		Printer<float>::stream(s, indent + " ", v.depth_units);

Adding depth units value to frame metadata #9154

Adding depth units value to frame metadata #9154

Conversation

nohayassin commented Jun 3, 2021

ev-mp Jun 6, 2021 • edited

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

ev-mp left a comment

Choose a reason for hiding this comment

ev-mp Jun 14, 2021 • edited

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

ev-mp left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

ev-mp Jun 6, 2021 •

edited

ev-mp Jun 14, 2021 •

edited