Split 2D+3D capture

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 $?
 

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_;

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)

zivid_camera/src/zivid_camera.cpp

@@ -15,6 +15,7 @@
 #include <boost/algorithm/string.hpp>
 #include <boost/predef.h>
 
+#include <future>
 #include <numeric>
 #include <sstream>
 #include <thread>
@@ -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<int>(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<int>(color_space)));
+  }
+}
+
 void ZividCamera::publishColorImage(const std_msgs::Header& header, const sensor_msgs::CameraInfoConstPtr& camera_info,
                                     const Zivid::Image<Zivid::ColorRGBA>& 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<Zivid::Frame> 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__);

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