Enabling depth registration does not align depth and color images for femto bolt driver

[Schteve-Earl]

Hey Ya'll,

I'm in the process of testing out a Femto Bolt camera, and I'm running into some issues regarding the alignment of the depth and color images that come from the ros driver. The depth image seems to be shifted from the color image by a constant amount, even though I've set the depth_registration parameter to "True". Now, I assume that the offset is due to the image sensor and the ToF sensor positioning on the actual camera, but I was under the impression that the depth_registration parameter would align both the color point cloud topic, as well as the image topics that are streaming from the driver. Is this a bug? Or did you intend for a different method of aligning the color and depth images?

[jian-dong]

Hi @Schteve-Earl I tested femto bolt, I set registration to True, and enable_d2c_viewer To True for testing. subscribe topic /camera/depth_to_color/image_raw on rviz2 . I think the result it aligned

[jian-dong]

/camera/depth_to_color/image_raw is depth to color overlay image.

[Schteve-Earl]

Thanks for your help, I didn't see the enable_d2v_viewer parameter. I see that the encoding for the the images coming from /camera/depth_to_color/image_raw is rgb8. How then is the depth encoded into the image? Which channels hold the depth data, and how do I access it?

[Schteve-Earl]

Additionally, I wanted to provide some insite into my issues. Here is an image from the /camera/depth_to_color/image_raw: However, when I turn it to a color pointcloud, you can clearly see the offset between the color and depth data:

Additionally, take a look at the colored point cloud constructed from the color image and the depth image: The orange is supposed to be a round ball. In an aligned system, the ball would fill the entire point cloud area. In my case, the image of the ball bleeds onto the background, and likewise the background is part of the elevated points. This is specifically the issue that I'm attempting to solve in the driver, before I attempt to solve it elsewhere.

[jian-dong]

Thank you for your feedback, @Schteve-Earl.

Regarding the enable_d2v_viewer parameter, it is indeed included in the launch file femto_bolt.launch.py. Here's a snippet of the relevant part of the Python launch file:

# ...
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
# ...

You mentioned the /camera/depth_to_color/image_raw topic and were curious about how depth data is encoded in the RGB8 format images it provides. In this setup, the /camera/depth_to_color/image_raw topic is used primarily for overlaying the images for testing purposes. Unfortunately, this means that direct access to depth data in these images isn't possible.

To clarify how depth and color images are overlapped, here's the relevant C++ code from the orbbec_camera namespace:

namespace orbbec_camera {
// D2CViewer class definition and constructor
// ...
void D2CViewer::messageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& rgb_msg,
                                const sensor_msgs::msg::Image::ConstSharedPtr& depth_msg) {
  // Check if the sizes of RGB and depth images match
  if (rgb_msg->width != depth_msg->width || rgb_msg->height != depth_msg->height) {
    // Log an error if they don't match
    RCLCPP_ERROR(logger_, "rgb and depth image size not match(%d, %d) vs (%d, %d)",
                 rgb_msg->width, rgb_msg->height, depth_msg->width, depth_msg->height);
    return;
  }

  // Convert RGB and depth images to OpenCV formats
  auto rgb_img_ptr = cv_bridge::toCvCopy(rgb_msg, sensor_msgs::image_encodings::RGB8);
  auto depth_img_ptr = cv_bridge::toCvCopy(depth_msg, sensor_msgs::image_encodings::TYPE_16UC1);

  // Process depth image and combine it with RGB image
  cv::Mat gray_depth, depth_img, d2c_img;
  depth_img_ptr->image.convertTo(gray_depth, CV_8UC1);
  cv::cvtColor(gray_depth, depth_img, cv::COLOR_GRAY2RGB);
  depth_img.setTo(cv::Scalar(255, 255, 0), depth_img);
  cv::bitwise_or(rgb_img_ptr->image, depth_img, d2c_img);

  // Publish the combined image
  sensor_msgs::msg::Image::SharedPtr d2c_msg =
      cv_bridge::CvImage(std_msgs::msg::Header(), sensor_msgs::image_encodings::RGB8, d2c_img)
          .toImageMsg();
  d2c_msg->header = rgb_msg->header;
  d2c_viewer_pub_->publish(*d2c_msg);
}
}  // namespace orbbec_camera

In this implementation, the depth image is first converted to a grayscale format, then to RGB, and finally overlaid with the RGB image using bitwise operations. However, the depth information is not directly accessible in the final overlaid image.

I hope this explanation helps clear up your questions. Let me know if there's anything else you'd like to know!

[jian-dong]

Additionally, I wanted to provide some insite into my issues. Here is an image from the /camera/depth_to_color/image_raw: However, when I turn it to a color pointcloud, you can clearly see the offset between the color and depth data:

Additionally, take a look at the colored point cloud constructed from the color image and the depth image: The orange is supposed to be a round ball. In an aligned system, the ball would fill the entire point cloud area. In my case, the image of the ball bleeds onto the background, and likewise the background is part of the elevated points. This is specifically the issue that I'm attempting to solve in the driver, before I attempt to solve it elsewhere.

Thank you for sharing the images and describing the issues you're experiencing with the alignment of color and depth data in your imaging setup. To better understand and address the problem, I need some additional details:

1. Resolution of Images: Could you please specify the resolutions of both the color and depth images? Knowing the exact resolution for each type is crucial, as differences in resolution can often lead to alignment issues.

2. Image Generation Process: How exactly were the two images you've shared generated? Understanding the steps or methods you used, including any specific software or settings, is important. If you followed a particular tutorial or documentation to generate these images, sharing that would be beneficial. This will help in replicating your setup and better analyzing the problem.

Once we have this information, it will be much easier to diagnose the issue and suggest potential solutions.

[jian-dong]

@zhonghong322 这个客户用了femto bolt 发现深度和彩色点云有偏移，我也不是很理解他遇到的问题，我们测试过D2C应该是对齐了的，我感觉是应该理解上出了什么偏差，有时间帮忙一起看看。 @lixby03

[Schteve-Earl]

Thanks for the writeup, that tells me what I need to know. I was in the process of creating a minimum testable example for you to try, but I was able to find a bug in my complete system that was causing the offset. Thank you for your help!

[jian-dong]

@Schteve-Earl If there is anything I can do for you, you can reopen this issue.