Realsense D435 align_depth

jespestana commented 5 years ago

When running the ROS node with the Realsense D435 and the align_depth parameter as "true", the depth images from the topics "aligned_depth_to_infra1" and "aligned_depth_to_infra2" are identical. I have even checked this by saving to disk synched images from both topics and checking their md5sums.

Expected behaviour:

Depth image "aligned_depth_to_infra1" aligned with infra1 projection center
Depth image "aligned_depth_to_infra2" aligned with infra2 projection center

Actual behaviour:

Both depth images are identical and aligned with infra1 projection center

System setup:

Ubuntu 16.04.5 LTS (xenial)
ROS Kinetic
Intel Realsense D435 - firmware version: 05.11.01.00
git@github.com:intel-ros/realsense.git - development branch - commit 3e25326b4128d37b15d1e11085dd2f19d3cf1a66 (Feb 19, 2019 )

doronhi commented 5 years ago

Thank you. I can confirm the bug and will keep you posted on the development.

jespestana commented 5 years ago

Thanks for looking into this.

I have 2 related question if I may ask:

Q1: Why is it that when the roslaunch-parameter align_depth is set to true, the timestamping cannot be done using frame_time = frame.get_timestamp() anymore? (In the code this is the same as setting the roslaunch-parameter enable_sync to true)

We tested the the synch between 2 D435 cameras using the roslaunch-parameters enable_sync and align_depth set to false, and the synch using the frame_time was very good, at least for short periods of time (precise to up to an error of around 7ms and repeatable over time).

Q2: I would like to report the following. We have set up a system with multiple cameras, and the initialization of the D435 realsense cameras is much more reliable when we perform a Hardware Reset using the realsense-viewer before starting our ROS architecture (we are also setting the roslaunch-parameter initial_reset to true).

doronhi commented 5 years ago

@jespestana, Q1: I'm not sure I understand you correctly. frame.get_timestamp() returns the hardware time - time in miliseconds since the device was started. Therefor is it aligned to ROS time.

enable_sync has nothing to do with multiple cameras. It is about the synchronization of different sensors (RGB, infrared) in the same device. Error of 7ms with 30 fps does not sounds so accurate to me. If you were looking for hardware synchronization between cameras and connecting them with a synchronization cable, you should set one of them to Slave mode. You can use rqt_reconfigure- camera/Stereo_Module/Inter_Cam_Sync_Mode (1-Master, 2-Slave)

Q2: Setting initial_reset:=true is suppose to give the same results as Hardware Reset in realsense-viewer. Do you mean to say that the system is less reliable if you only activate 1 of the 2 options (reset with realsense-viewer and reset with initial_reset:=true)?

Regarding the align_depth issue, this requires modification to the underlying librealsense library and might take a while. Can you explain please why do you need that option, to project depth on infra2? As far as I can tell, all the information is in infra1 and as for areas that are seen in infra2 and not in infra1 there is no depth available anyway. Since naturally I don't want any malfunctioning options in the driver, I am thinking of removing the option of aligning depth to infra2 altogether. Can I ask for your thought on the subject?

jespestana commented 5 years ago

Regarding the align_depth issue

[...] Since naturally I don't want any malfunctioning options in the driver, I am thinking of removing the option of aligning depth to infra2 altogether.

In my opinion, since the aligned_depth_to_infra2 contains no new information, the solution that you are proposing is good.

For now, I just developed the software to acquire all available data from the realsense. I was even considering not saving to disk the aligned_depth_to_X images, since I believe that all related data is contained in the depth image already.

About your answer to Q1

Thanks for the information about the synchronization of multiple cameras.

Regarding my original question Q1, I do not understand why setting align_depth to true automatically activates enable_sync. enable_sync == true then causes the software to ignore the frame_time. I am refferring to the following 2 codes snippets (branch development commit: 80df5e57240d86e4f3026aaf24659057d2882bf4 - 20th Feb 2019).

Lines 337-339 of base_realsense_node.cpp:

    _pnh.param("enable_sync", _sync_frames, SYNC_FRAMES);
    if (_pointcloud || _align_depth || _filters_str.size() > 0)
        _sync_frames = true;

At the beginning of the function void BaseRealSenseNode::frame_callback(rs2::frame frame), lines 1226-1245 of base_realsense_node.cpp:

        double frame_time = frame.get_timestamp();

        // We compute a ROS timestamp which is based on an initial ROS time at point of first frame,
        // and the incremental timestamp from the camera.
        // In sync mode the timestamp is based on ROS time
        bool placeholder_false(false);
        if (_is_initialized_time_base.compare_exchange_strong(placeholder_false, true) )
        {
            setBaseTime(frame_time, RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME == frame.get_frame_timestamp_domain());
        }

        ros::Time t;
        if (_sync_frames)
        {
            t = ros::Time::now();
        }
        else
        {
            t = ros::Time(_ros_time_base.toSec()+ (/*ms*/ frame_time - /*ms*/ _camera_time_base) / /*ms to seconds*/ 1000);
        }

About your answer to Q2

Q2: Setting initial_reset:=true is suppose to give the same results as Hardware Reset in realsense-viewer. Do you mean to say that the system is less reliable if you only activate 1 of the 2 options (reset with realsense-viewer and reset with initial_reset:=true)?

I have only tested doing [a] only initial_reset:=true and [b] Hardware Reset with realsense-viewer followed by initial_reset:=true. Note: my implementation uses the nodelet version for 2 realsense cameras D435.

In my experience, [b] makes the system start properly more often (maybe even always).

If it would be of any help I could test [c] only Hardware Reset with realsense-viewer.

doronhi commented 5 years ago

The code sets align_depth to true automatically because when you align one image to another you want them to be taken as close as possible to the same moment. Enabling sync_mode does that - It creates a set of images from approximately the same time.

In both cases, enabling sync or not, the time is in the ROS base time. If we deal with 1 frame at a time, we can take it's time difference from the first frame of the same sensor, add to the base ros time and that would be it's time stamp. When we are deal with a set of frames, we want to give all of them the same timestamp so as to make it easier to relate to them as a bunch. In that case, which frame will serve as a time source? We can't assume that if we had color frame in the first set we'll always have a color frame in the set. Therefor, the fallback was to use ros::Time directly.

jespestana commented 5 years ago

Ok, that makes sense. Thanks for the information!

From my side and given the information you have provided about the original issue, I believe that this issue can be closed.

doronhi commented 5 years ago

Thanks. Just one question before we close, if you don't mind. When you said the system was less reliable, what would happen when it was NOT working ok? What would the symptoms be? Error messages, etc. I mean, what problem was solved by using the realsense-viewer's hardware reset option?

jespestana commented 5 years ago

When you said the system was less reliable, what would happen when it was NOT working ok? What would the symptoms be? Error messages, etc. I mean, what problem was solved by using the realsense-viewer's hardware reset option?

Unfortunately the system is being used by an external, so I cannot give a very precise answer right now...

With an older version of the firmware (~Oct/Nov 2018), what happened is that the D435 would not start-up properly and the nodelet manager would crash. After the crash, the D435 device would not be recognized as an USB device. For instance, the device would not be listed when using the terminal command lsusb --tree. To get the system back to work the computer would need to be restarted.

With the new firmware (Intel Realsense D435 - firmware version: 05.11.01.00), what happens is that the image streams are not published by the nodelet, and the nodelet manager does not crash. As far as I can remember the D435 devices would always still appear when using the terminal command lsusb --tree. To solve the issue I would stop the acquisition system (terminate all nodes), and go through the Hardware Reset procedure using the realsense-viewer. The system usually starts properly after doing this. Note: as explained above we are launching the D435 nodelets using the parameter initial_reset set to true.

doronhi commented 5 years ago

Thank you. It is related to a known firmware problem. I am working to make the next realsense2_wrapper release more robust to this kind of problem. Thanks for the information.

mikepurvis commented 5 years ago

@jespestana @doronhi Hey, we're finding a similar behaviour, where the Nov 2018 firmware would often not enumerate and require a power cycle (or re-plug), whereas the 5.11.1.100 firmware requires software resets to get it into an operational state. Is the plan to build that into the driver? Are you interested in PRs to assist with that or should we just wait it out?

doronhi commented 5 years ago

If its not too much trouble, I believe it would be beneficial for me to have a PR.

IntelRealSense / realsense-ros

Realsense D435 align_depth #627