From 317f88cd54059b716c69335ed91fc491817b8774 Mon Sep 17 00:00:00 2001 From: SharedCSU Date: Mon, 30 Jun 2025 16:38:31 +0200 Subject: [PATCH] Split 2D+3D capture Zivid SDK will capture both 2D and 3D if `Zivid::Settings` contain `Zivid::Settings::color`. However, it will not return from the capture until both 2D and 3D has been acquired. In order to get 2D data as fast as possible we split the capture into two separate calls. This allows us to publish the 2D color image as soon as it is ready. This has no impact on the subscriber, other than in which order the topics arrive. Before: `acquisition_done` -> `color/image_color` -> `points/xyz` After: `color/image_color` -> `acquisition_done` -> `points/xyz` The order of `color/image_color` and `acquisition_done` depends on the 3D acquisition settings and the processing speed of 2D color. Note! Previously the resolution of the 2D image, when published as part of the 3D capture service, was tied to the 3D point cloud. That also applied to intrinsics. Now it will only reflect the 2D settings. In other words, in order to have 1-to-1 mapping between the 2D color image and the point cloud one must set the same Zivid::Settings::Sampling in both. In other words: ```yml Settings: Color: __version__: 7 Settings2D: Sampling: Color: rgb Pixel: all ``` must equal ```yml Settings: Sampling: Color: rgb Pixel: all ``` in order to have the same resolution on both. This also means that there is no path to publish the point cloud resolution mapped color image, which was previously published even when `Zivid::Settings2D::Sampling` != `Zivid::Settings::Sampling` In this commit we also cache the intrinsics in order to unblock publishing 2D color image. This is because calculating intrinsics requires the camera. --- .../setup/setup_code_analysis.sh | 8 + zivid_camera/include/zivid_camera.h | 3 + zivid_camera/src/node.cpp | 6 +- zivid_camera/src/zivid_camera.cpp | 104 ++++++++----- zivid_samples/settings/camera_settings.yml | 140 ++++++++++++------ .../sample_capture_with_settings_from_yml.cpp | 39 ++++- 6 files changed, 218 insertions(+), 82 deletions(-) diff --git a/continuous-integration/setup/setup_code_analysis.sh b/continuous-integration/setup/setup_code_analysis.sh index 0c222537..b0f6a911 100755 --- a/continuous-integration/setup/setup_code_analysis.sh +++ b/continuous-integration/setup/setup_code_analysis.sh @@ -10,6 +10,14 @@ function apt-yes { apt-get --assume-yes "$@" } +apt-yes install curl gnupg2 lsb-release || exit $? + +curl -fsSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc \ + | gpg --dearmor -o /usr/share/keyrings/ros-archive-keyring.gpg + +echo "deb [signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" \ + > /etc/apt/sources.list.d/ros-latest.list + apt-yes update || exit $? apt-yes dist-upgrade || exit $? diff --git a/zivid_camera/include/zivid_camera.h b/zivid_camera/include/zivid_camera.h index a44d9805..6d1c60f4 100644 --- a/zivid_camera/include/zivid_camera.h +++ b/zivid_camera/include/zivid_camera.h @@ -76,6 +76,7 @@ class ZividCamera void publishPointCloudXYZ(const std_msgs::Header& header, const Zivid::PointCloud& point_cloud); void publishPointCloudXYZRGBA(const std_msgs::Header& header, const Zivid::PointCloud& point_cloud, ColorSpace color_space); + void publishColorImageFromFrame2D(const Zivid::Frame2D& frame2D); void publishColorImage(const std_msgs::Header& header, const sensor_msgs::CameraInfoConstPtr& camera_info, const Zivid::PointCloud& point_cloud, ColorSpace color_space); void publishColorImage(const std_msgs::Header& header, const sensor_msgs::CameraInfoConstPtr& camera_info, @@ -90,6 +91,7 @@ class ZividCamera sensor_msgs::CameraInfoConstPtr makeCameraInfo(const std_msgs::Header& header, std::size_t width, std::size_t height, const Zivid::CameraIntrinsics& intrinsics); ColorSpace colorSpace() const; + void updateIntrinsics2D(const Zivid::CameraIntrinsics& intrinsics); IntrinsicsSource intrinsicsSource() const; // using Capture3DSettingsController = @@ -133,6 +135,7 @@ class ZividCamera Zivid::Camera camera_; Zivid::Settings current_settings_; Zivid::Settings2D current_settings_2d_; + Zivid::CameraIntrinsics current2Dintrinsics_; std::string frame_id_; unsigned int header_seq_; diff --git a/zivid_camera/src/node.cpp b/zivid_camera/src/node.cpp index de736280..12a23c89 100644 --- a/zivid_camera/src/node.cpp +++ b/zivid_camera/src/node.cpp @@ -37,7 +37,11 @@ int main(int argc, char** argv) } ROS_INFO("Successfully loaded nodelet '%s'", nodelet_name); - ros::spin(); + // ros::spin(); + ros::AsyncSpinner spinner(3); // Use 2 threads or more, depends on workload + spinner.start(); + + ros::waitForShutdown(); return EXIT_SUCCESS; } catch (const std::exception& e) diff --git a/zivid_camera/src/zivid_camera.cpp b/zivid_camera/src/zivid_camera.cpp index 3b959719..1ac58596 100644 --- a/zivid_camera/src/zivid_camera.cpp +++ b/zivid_camera/src/zivid_camera.cpp @@ -15,6 +15,7 @@ #include #include +#include #include #include #include @@ -317,6 +318,8 @@ ZividCamera::ZividCamera(ros::NodeHandle& nh, ros::NodeHandle& priv) ROS_INFO_STREAM("Connected to camera '" << camera_.info().serialNumber() << "'"); setCameraStatus(CameraStatus::Connected); + updateIntrinsics2D(Zivid::Experimental::Calibration::intrinsics(camera_, current_settings_2d_)); + camera_connection_keepalive_timer_ = nh_.createTimer(ros::Duration(10), &ZividCamera::onCameraConnectionKeepAliveTimeout, this); @@ -471,7 +474,6 @@ bool ZividCamera::capture2DServiceHandler(Capture2D::Request&, Capture2D::Respon serviceHandlerHandleCameraConnectionLoss(); - const auto color_space = colorSpace(); const auto settings2D = current_settings_2d_; // capture_2d_settings_controller_->zividSettings(); if (settings2D.acquisitions().isEmpty()) @@ -482,34 +484,14 @@ bool ZividCamera::capture2DServiceHandler(Capture2D::Request&, Capture2D::Respon ROS_INFO("Capturing 2D with %zd acquisition(s)", settings2D.acquisitions().size()); ROS_DEBUG_STREAM(settings2D); auto frame2D = camera_.capture(settings2D); + updateIntrinsics2D(Zivid::Experimental::Calibration::intrinsics(camera_, settings2D)); if (shouldPublishAcquisitionDone()) { publishAcquisitionDone(makeHeader()); } if (shouldPublishColorImg()) { - const auto header = makeHeader(); - const auto intrinsics = Zivid::Experimental::Calibration::intrinsics(camera_, settings2D); - switch (color_space) - { - case ColorSpace::sRGB: { - auto image = frame2D.imageRGBA_SRGB(); - const auto camera_info = makeCameraInfo(header, image.width(), image.height(), intrinsics); - ROS_DEBUG("Publishing image with 'srgb' color space"); - publishColorImage(header, camera_info, image); - } - break; - case ColorSpace::LinearRGB: { - auto image = frame2D.imageRGBA(); - const auto camera_info = makeCameraInfo(header, image.width(), image.height(), intrinsics); - ROS_DEBUG("Publishing image with 'linear' color space"); - publishColorImage(header, camera_info, image); - } - break; - default: - throw std::runtime_error("Internal error: Unknown color space value " + - std::to_string(static_cast(color_space))); - } + publishColorImageFromFrame2D(frame2D); } return true; } @@ -596,13 +578,12 @@ void ZividCamera::publishFrame(const Zivid::Frame& frame) const bool publish_acquisition_done = shouldPublishAcquisitionDone(); const bool publish_points_xyz = shouldPublishPointsXYZ(); const bool publish_points_xyzrgba = shouldPublishPointsXYZRGBA(); - const bool publish_color_img = shouldPublishColorImg(); const bool publish_depth_img = shouldPublishDepthImg(); const bool publish_snr_img = shouldPublishSnrImg(); const bool publish_normals_xyz = shouldPublishNormalsXYZ(); - if (publish_acquisition_done || publish_points_xyz || publish_points_xyzrgba || publish_color_img || - publish_depth_img || publish_snr_img || publish_normals_xyz) + if (publish_acquisition_done || publish_points_xyz || publish_points_xyzrgba || publish_depth_img || + publish_snr_img || publish_normals_xyz) { const auto color_space = colorSpace(); const auto header = makeHeader(); @@ -627,17 +608,17 @@ void ZividCamera::publishFrame(const Zivid::Frame& frame) { publishPointCloudXYZRGBA(header, point_cloud, color_space); } - if (publish_color_img || publish_depth_img || publish_snr_img) + if (publish_depth_img || publish_snr_img) { auto intrinsics = [&] { const auto intrinsics_source = intrinsicsSource(); switch (intrinsics_source) { case IntrinsicsSource::Camera: - ROS_INFO("Using camera intrinsics for publishing images"); + ROS_DEBUG("Using camera intrinsics for publishing images"); return Zivid::Experimental::Calibration::intrinsics(camera_, frame.settings()); case IntrinsicsSource::Frame: - ROS_INFO("Using frame intrinsics for publishing images"); + ROS_DEBUG("Using frame intrinsics for publishing images"); return Zivid::Experimental::Calibration::estimateIntrinsics(frame); default: throw std::runtime_error("Internal error: Unknown intrinsics source value " + @@ -646,10 +627,6 @@ void ZividCamera::publishFrame(const Zivid::Frame& frame) }(); const auto camera_info = makeCameraInfo(header, point_cloud.width(), point_cloud.height(), intrinsics); - if (publish_color_img) - { - publishColorImage(header, camera_info, point_cloud, color_space); - } if (publish_depth_img) { publishDepthImage(header, camera_info, point_cloud); @@ -785,6 +762,34 @@ void ZividCamera::publishColorImage(const std_msgs::Header& header, const sensor color_image_publisher_.publish(image, camera_info); } +void ZividCamera::publishColorImageFromFrame2D(const Zivid::Frame2D& frame2D) +{ + ROS_DEBUG("Publishing color image from Zivid::Frame2D"); + const auto color_space = colorSpace(); + const auto header = makeHeader(); + const auto settings2D = frame2D.settings(); + switch (color_space) + { + case ColorSpace::sRGB: { + auto image = frame2D.imageRGBA_SRGB(); + const auto camera_info = makeCameraInfo(header, image.width(), image.height(), current2Dintrinsics_); + ROS_DEBUG("Publishing image with 'srgb' color space"); + publishColorImage(header, camera_info, image); + } + break; + case ColorSpace::LinearRGB: { + auto image = frame2D.imageRGBA(); + const auto camera_info = makeCameraInfo(header, image.width(), image.height(), current2Dintrinsics_); + ROS_DEBUG("Publishing image with 'linear' color space"); + publishColorImage(header, camera_info, image); + } + break; + default: + throw std::runtime_error("Internal error: Unknown color space value " + + std::to_string(static_cast(color_space))); + } +} + void ZividCamera::publishColorImage(const std_msgs::Header& header, const sensor_msgs::CameraInfoConstPtr& camera_info, const Zivid::Image& image) { @@ -897,9 +902,30 @@ Zivid::Frame ZividCamera::invokeCaptureAndPublishFrame() throw std::runtime_error("capture called with 0 enabled acquisitions!"); } - ROS_INFO("Capturing with %zd acquisition(s)", settings.acquisitions().size()); - ROS_DEBUG_STREAM(settings); - const auto frame = camera_.capture(settings); + ROS_DEBUG("Capturing with %zd acquisition(s)", settings.acquisitions().size()); + Zivid::Frame2D frame2D; + bool should_publish_color_img = shouldPublishColorImg(); + if (settings.color().hasValue() && should_publish_color_img) + { + ROS_DEBUG_STREAM("Capturing 2D as part of 2D+3D. " << color_image_publisher_.getNumSubscribers() + << " subscribers."); + frame2D = camera_.capture2D(settings); + updateIntrinsics2D(Zivid::Experimental::Calibration::intrinsics(camera_, settings)); + } + else if (should_publish_color_img) + { + ROS_DEBUG("No color image to publish from Zivid::Frame2D, since settings.color() is not set."); + should_publish_color_img = false; + } + ROS_DEBUG("Capturing 3D as part of 2D+3D"); + std::future frame3D_future = + std::async(std::launch::async, &decltype(camera_)::capture3D, &camera_, settings); + if (should_publish_color_img) + { + ROS_DEBUG("Publishing color image from Zivid::Frame2D in separate thread"); + publishColorImageFromFrame2D(frame2D); + } + const auto frame = frame3D_future.get(); publishFrame(frame); return frame; } @@ -916,6 +942,12 @@ ColorSpace ZividCamera::colorSpace() const return parameterStringToEnum(ParamNames::color_space, color_space_str, color_space_name_value_map_); } +void ZividCamera::updateIntrinsics2D(const Zivid::CameraIntrinsics& intrinsics) +{ + ROS_DEBUG_STREAM(__func__); + current2Dintrinsics_ = intrinsics; +} + IntrinsicsSource ZividCamera::intrinsicsSource() const { ROS_DEBUG_STREAM(__func__); diff --git a/zivid_samples/settings/camera_settings.yml b/zivid_samples/settings/camera_settings.yml index 635c095e..966cf37f 100644 --- a/zivid_samples/settings/camera_settings.yml +++ b/zivid_samples/settings/camera_settings.yml @@ -1,48 +1,100 @@ -__version__: - serializer: 1 - data: 10 +__version__: 27 Settings: - Acquisitions: - - Acquisition: - Aperture: 5.66 - Brightness: 1.5 - ExposureTime: 6500 - Gain: 1 - Diagnostics: - Enabled: no - Experimental: - Engine: phase - Processing: + Acquisitions: + - Acquisition: + Aperture: 2.83 + Brightness: 1.8 + ExposureTime: 40000 + Gain: 1 + - Acquisition: + Aperture: 2.83 + Brightness: 1.8 + ExposureTime: 40000 + Gain: 1 Color: - Balance: - Blue: 1 - Green: 1 - Red: 1 - Experimental: - ToneMapping: - Enabled: hdrOnly - Gamma: 1 - Filters: - Experimental: - ContrastDistortion: - Correction: + __version__: 7 + Settings2D: + Acquisitions: + - Acquisition: + Aperture: 4.0 + Brightness: 1.8 + ExposureTime: 2000 + Gain: 1 + Processing: + Color: + Balance: + Blue: 1 + Green: 1 + Red: 1 + Experimental: + Mode: automatic + Gamma: 1 + Sampling: + Color: rgb + Pixel: all + Diagnostics: + Enabled: no + Engine: stripe + Processing: + Color: + Balance: + Blue: 1 + Green: 1 + Red: 1 + Experimental: + Mode: automatic + Gamma: 1 + Filters: + Cluster: + Removal: + Enabled: yes + MaxNeighborDistance: 7 + MinArea: 500 + Experimental: + ContrastDistortion: + Correction: + Enabled: yes + Strength: 0.3 + Removal: + Enabled: no + Threshold: 0.3 + Hole: + Repair: + Enabled: yes + HoleSize: 0.2 + Strictness: 2 + Noise: + Removal: + Enabled: yes + Threshold: 3 + Repair: + Enabled: no + Suppression: + Enabled: no + Outlier: + Removal: + Enabled: yes + Threshold: 20 + Reflection: + Removal: + Enabled: yes + Mode: global + Smoothing: + Gaussian: + Enabled: yes + Sigma: 4 + Resampling: + Mode: __not_set__ + RegionOfInterest: + Box: Enabled: no - Strength: 0.4 - Removal: + Extents: [-10, 100] + PointA: [0, 0, 0] + PointB: [0, 0, 0] + PointO: [0, 0, 0] + Depth: Enabled: no - Threshold: 0.5 - Noise: - Removal: - Enabled: yes - Threshold: 7 - Outlier: - Removal: - Enabled: yes - Threshold: 5 - Reflection: - Removal: - Enabled: no - Smoothing: - Gaussian: - Enabled: no - Sigma: 1.5 + Range: [300, 1300] + Sampling: + Color: rgb + Pixel: all diff --git a/zivid_samples/src/sample_capture_with_settings_from_yml.cpp b/zivid_samples/src/sample_capture_with_settings_from_yml.cpp index b59e7387..64aaa396 100644 --- a/zivid_samples/src/sample_capture_with_settings_from_yml.cpp +++ b/zivid_samples/src/sample_capture_with_settings_from_yml.cpp @@ -2,6 +2,7 @@ #include #include #include +#include #include #include #include @@ -19,6 +20,21 @@ namespace { const ros::Duration default_wait_duration{ 30 }; +auto waitForPublisher = [](const ros::Subscriber& sub, ros::Duration timeout) { + ros::Time start = ros::Time::now(); + ros::Rate rate(10); + while ((ros::Time::now() - start) < timeout) + { + if (sub.getNumPublishers() > 0) + { + return true; + } + ros::spinOnce(); + rate.sleep(); + } + return false; +}; + void capture() { ROS_INFO("Calling capture service"); @@ -26,6 +42,20 @@ void capture() CHECK(ros::service::call("/zivid_camera/capture", capture)); } +void on_points(const sensor_msgs::PointCloud2ConstPtr&) +{ + ROS_INFO("PointCloud received"); +} + +void on_image_color(const sensor_msgs::ImageConstPtr&) +{ + ROS_INFO("2D color image received"); +} + +void on_acquisition_done(const std_msgs::HeaderConstPtr& header) +{ + ROS_INFO("Acquisition done: %s", header->frame_id.c_str()); +} } // namespace int main(int argc, char** argv) @@ -37,9 +67,16 @@ int main(int argc, char** argv) CHECK(ros::service::waitForService("/zivid_camera/capture", default_wait_duration)); - ros::AsyncSpinner spinner(1); + ros::AsyncSpinner spinner(3); spinner.start(); + auto points_sub = n.subscribe("/zivid_camera/points/xyzrgba", 1, on_points); + CHECK(waitForPublisher(points_sub, default_wait_duration)); + auto image_color_sub = n.subscribe("/zivid_camera/color/image_color", 1, on_image_color); + CHECK(waitForPublisher(image_color_sub, default_wait_duration)); + auto acquisition_done_sub = n.subscribe("/zivid_camera/acquisition_done", 1, on_acquisition_done); + CHECK(waitForPublisher(acquisition_done_sub, default_wait_duration)); + std::string samples_path = ros::package::getPath("zivid_samples"); std::string settings_path = samples_path + "/settings/camera_settings.yml"; ROS_INFO("Loading settings from: %s", settings_path.c_str());