Adding depth units value to frame metadata by nohayassin · Pull Request #9154 · realsenseai/librealsense

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

Track on DSO-16066

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

+    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

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

-                }
-            }
+            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

        };

-        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

    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

-                    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

        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

            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

        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

+        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

        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

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


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

        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


                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


    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

            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

            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

        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

+                    // 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

-        // 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

    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

        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

            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

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


Shouldn't be needed

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

        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

      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

            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

+        {
+            _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

+        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

                _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

-        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

        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

-        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

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


Print only for depth frame

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

    }
-    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


   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

            _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

        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

      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 suggested 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 suggested 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 suggested 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 suggested changes Jun 16, 2021

View reviewed changes

fixes

fde3cd6

ev-mp suggested changes Jun 20, 2021

View reviewed changes

fixes

0065af3

ev-mp suggested 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 realsenseai: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);

Uh oh!

Conversation

nohayassin commented Jun 3, 2021

Uh oh!

ev-mp Jun 6, 2021 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

ev-mp left a comment

Choose a reason for hiding this comment

Uh oh!

ev-mp Jun 14, 2021 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

ev-mp Jun 6, 2021 •

edited

Loading

ev-mp Jun 14, 2021 •

edited

Loading