Add ability to estimate intrinsics from frame

README.md

@@ -220,6 +220,21 @@ Or, if using `roslaunch` specify the parameter using `<param>`:
 > [2D Color Spaces and Output Formats](https://support.zivid.com/en/latest/reference-articles/color-spaces-and-output-formats.html)
 > for more details.
 
+`intrinsics_source` (string, default: "camera")
+> Specify how intrinsics are determined when publishing images from 3D captures. Valid values:
+>
+> - `camera`: Use hard-coded camera intrinsics. These intrinsics are given for a single aperture and a
+>    single temperature, and will therefore not be as accurate as the intrinsics estimated from the frame.
+> - `frame`: Estimate the intrinsics from the captured 3D frame. This gives more accurate results at the
+>    cost of additional computation time.
+>
+> In particular, this parameter affects data published on the topics [color/camera_info](#colorcamera_info),
+> [depth/camera_info](#depthcamera_info), and [snr/camera_info](#snrcamera_info), after performing a 3D capture. The
+> 2D-only captures will always use the hard-coded camera intrinsics. Please see the Zivid knowledge base on
+> [Camera Intrinsics](https://support.zivid.com/en/latest/reference-articles/camera-intrinsics.html) for more details.
+>
+> See [Sample Intrinsics](#sample-intrinsics) for code example.
+
 ## Services
 
 ### capture_assistant/suggest_settings
@@ -324,7 +339,7 @@ performance reasons no messages are generated or sent on topics with zero active
 ### color/camera_info
 [sensor_msgs/CameraInfo](http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html)
 
-Camera calibration and metadata.
+Camera calibration and metadata. See also parameter [`intrinsics_source`](#launch-parameters-advanced).
 
 ### color/image_color
 [sensor_msgs/Image](http://docs.ros.org/api/sensor_msgs/html/msg/Image.html)
@@ -336,7 +351,7 @@ is always 255.
 ### depth/camera_info
 [sensor_msgs/CameraInfo](http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html)
 
-Camera calibration and metadata.
+Camera calibration and metadata. See also parameter [`intrinsics_source`](#launch-parameters-advanced).
 
 ### depth/image
 [sensor_msgs/Image](http://docs.ros.org/api/sensor_msgs/html/msg/Image.html)
@@ -361,7 +376,7 @@ the RGBA values.
 ### snr/camera_info
 [sensor_msgs/CameraInfo](http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html)
 
-Camera calibration and metadata.
+Camera calibration and metadata. See also parameter [`intrinsics_source`](#launch-parameters-advanced).
 
 ### snr/image
 [sensor_msgs/Image](http://docs.ros.org/api/sensor_msgs/html/msg/Image.html)
@@ -452,6 +467,25 @@ rosrun zivid_samples sample_capture_with_settings_from_yml_cpp
 rosrun zivid_samples sample_capture_with_settings_from_yml.py
 ```
 
+### Sample Intrinsics
+
+This sample performs 3D captures and prints the published camera intrinsics with different settings. This sample shows
+how to set the [`intrinsics_source`](#launch-parameters-advanced) parameter, and how to subscribe to the
+[color/camera_info](#colorcamera_info) topic. Please see the Zivid knowledge base on
+[Camera Intrinsics](https://support.zivid.com/en/latest/reference-articles/camera-intrinsics.html) for more details.
+
+Source code: [C++](./zivid_samples/src/sample_intrinsics.cpp), [Python](./zivid_samples/scripts/sample_intrinsics.py)
+
+```bash
+ros2 launch zivid_samples sample.launch sample:=sample_intrinsics_cpp
+ros2 launch zivid_samples sample.launch sample:=sample_intrinsics.py
+```
+Using ros2 run (when `zivid_camera` node is already running):
+```bash
+ros2 run zivid_samples sample_intrinsics_cpp
+ros2 run zivid_samples sample_intrinsics.py
+```
+
 ## Frequently Asked Questions
 
 ### How to visualize the output from the camera in rviz

continuous-integration/run_code_analysis_in_docker.sh

@@ -7,8 +7,18 @@ fi
 
 echo "Starting $(basename $0) with CI_TEST_OS=$CI_TEST_OS"
 
+echo "Fixing expired GPG key before running apt-get update"
+mkdir -p ./continuous-integration/setup/keyrings
+curl -fsSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc \
+  | gpg --dearmor -o ./continuous-integration/setup/keyrings/ros-archive-keyring.gpg
+
+echo "Running the code analysis script in a Docker container"
 docker run \
     --volume $PWD:/host  \
+    --volume $PWD/continuous-integration/setup/keyrings/ros-archive-keyring.gpg:/usr/share/keyrings/ros1-latest-archive-keyring.gpg \
     --workdir /host/continuous-integration  \
     $CI_TEST_OS  \
     bash -c "./code_analysis.sh" || exit $?
+
+echo "Cleaning up keyrings"
+rm -rf ./continuous-integration/setup/keyrings

zivid_camera/include/zivid_camera.h

@@ -35,6 +35,12 @@ enum class ColorSpace
   LinearRGB,
 };
 
+enum class IntrinsicsSource
+{
+  Camera,
+  Frame,
+};
+
 class ZividCamera
 {
 public:
@@ -84,6 +90,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;
+  IntrinsicsSource intrinsicsSource() const;
 
   // using Capture3DSettingsController =
   //    CaptureSettingsController<Zivid::Settings, SettingsConfig, SettingsAcquisitionConfig>;
@@ -93,6 +100,7 @@ class ZividCamera
   ros::NodeHandle nh_;
   ros::NodeHandle priv_;
   std::map<std::string, ColorSpace> color_space_name_value_map_;
+  std::map<std::string, IntrinsicsSource> intrinsics_source_name_value_map_;
   ros::Timer camera_connection_keepalive_timer_;
   CameraStatus camera_status_;
   bool use_latched_publisher_for_acquisition_done_;

zivid_camera/src/zivid_camera.cpp

@@ -162,6 +162,7 @@ constexpr auto file_camera_path = "file_camera_path";
 constexpr auto frame_id = "frame_id";
 constexpr auto update_firmware_automatically = "update_firmware_automatically";
 constexpr auto color_space = "color_space";
+constexpr auto intrinsics_source = "intrinsics_source";
 }  // namespace ParamNames
 
 ZividCamera::ZividCamera(ros::NodeHandle& nh, ros::NodeHandle& priv)
@@ -179,6 +180,10 @@ ZividCamera::ZividCamera(ros::NodeHandle& nh, ros::NodeHandle& priv)
     {"srgb", zivid_camera::ColorSpace::sRGB},
     {"linear_rgb", zivid_camera::ColorSpace::LinearRGB},
   }
+  , intrinsics_source_name_value_map_{
+    {"camera", zivid_camera::IntrinsicsSource::Camera},
+    {"frame", zivid_camera::IntrinsicsSource::Frame},
+  }
   , zivid_(makeZividApplication())
   , header_seq_(0)
 {
@@ -222,6 +227,14 @@ ZividCamera::ZividCamera(ros::NodeHandle& nh, ros::NodeHandle& priv)
     priv_.setParam(ParamNames::color_space, color_space);
   }
 
+  std::string intrinsics_source;
+  if (!priv_.getParam(ParamNames::intrinsics_source, intrinsics_source))
+  {
+    ROS_INFO("No intrinsics source parameter found. Defaulting to 'camera'");
+    intrinsics_source = "camera";
+    priv_.setParam(ParamNames::intrinsics_source, intrinsics_source);
+  }
+
   priv_.param<bool>("use_latched_publisher_for_points_xyz", use_latched_publisher_for_points_xyz_, false);
   priv_.param<bool>("use_latched_publisher_for_points_xyzrgba", use_latched_publisher_for_points_xyzrgba_, false);
   priv_.param<bool>("use_latched_publisher_for_color_image", use_latched_publisher_for_color_image_, false);
@@ -476,7 +489,7 @@ bool ZividCamera::capture2DServiceHandler(Capture2D::Request&, Capture2D::Respon
   if (shouldPublishColorImg())
   {
     const auto header = makeHeader();
-    const auto intrinsics = Zivid::Experimental::Calibration::intrinsics(camera_);
+    const auto intrinsics = Zivid::Experimental::Calibration::intrinsics(camera_, settings2D);
     switch (color_space)
     {
       case ColorSpace::sRGB: {
@@ -616,7 +629,21 @@ void ZividCamera::publishFrame(const Zivid::Frame& frame)
     }
     if (publish_color_img || publish_depth_img || publish_snr_img)
     {
-      const auto intrinsics = Zivid::Experimental::Calibration::intrinsics(camera_);
+      auto intrinsics = [&] {
+        const auto intrinsics_source = intrinsicsSource();
+        switch (intrinsics_source)
+        {
+          case IntrinsicsSource::Camera:
+            ROS_INFO("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");
+            return Zivid::Experimental::Calibration::estimateIntrinsics(frame);
+          default:
+            throw std::runtime_error("Internal error: Unknown intrinsics source value " +
+                                     std::to_string(static_cast<int>(intrinsics_source)));
+        }
+      }();
       const auto camera_info = makeCameraInfo(header, point_cloud.width(), point_cloud.height(), intrinsics);
 
       if (publish_color_img)
@@ -889,4 +916,16 @@ ColorSpace ZividCamera::colorSpace() const
   return parameterStringToEnum(ParamNames::color_space, color_space_str, color_space_name_value_map_);
 }
 
+IntrinsicsSource ZividCamera::intrinsicsSource() const
+{
+  ROS_DEBUG_STREAM(__func__);
+
+  std::string intrinsics_source_str;
+  if (!priv_.getParam(ParamNames::intrinsics_source, intrinsics_source_str))
+  {
+    throw std::runtime_error("Failed to get parameter 'intrinsics_source'");
+  }
+  return parameterStringToEnum(ParamNames::intrinsics_source, intrinsics_source_str, intrinsics_source_name_value_map_);
+}
+
 }  // namespace zivid_camera

zivid_samples/CMakeLists.txt

@@ -47,6 +47,7 @@ endfunction()
 #register_cpp_sample(NAME sample_capture_2d_cpp SRC src/sample_capture_2d.cpp)
 register_cpp_sample(NAME sample_capture_assistant_cpp SRC src/sample_capture_assistant.cpp)
 register_cpp_sample(NAME sample_capture_with_settings_from_yml_cpp SRC src/sample_capture_with_settings_from_yml.cpp)
+register_cpp_sample(NAME sample_intrinsics_cpp SRC src/sample_intrinsics.cpp)
 
 ####################
 ## Python Samples ##
@@ -71,6 +72,7 @@ endfunction()
 #register_python_sample(SRC scripts/sample_capture_2d.py)
 register_python_sample(SRC scripts/sample_capture_assistant.py)
 register_python_sample(SRC scripts/sample_capture_with_settings_from_yml.py)
+register_python_sample(SRC scripts/sample_intrinsics.py)
 
 ####################
 ## Launch scripts ##

zivid_samples/scripts/sample_intrinsics.py

@@ -0,0 +1,91 @@
+#!/usr/bin/env python
+
+import rospy
+import rospkg
+from zivid_camera.srv import *
+from sensor_msgs.msg import CameraInfo
+
+
+class Sample:
+    def __init__(self):
+        rospy.init_node("sample_capture_with_settings_from_yml_py", anonymous=True)
+
+        rospy.loginfo("Starting sample_capture_with_settings_from_yml.py")
+
+        rospy.wait_for_service("/zivid_camera/capture", 30.0)
+
+        self.capture_service = rospy.ServiceProxy("/zivid_camera/capture", Capture)
+
+        rospy.Subscriber(
+            "/zivid_camera/color/camera_info", CameraInfo, self.on_color_camera_info
+        )
+        rospy.Subscriber(
+            "/zivid_camera/depth/camera_info", CameraInfo, self.on_depth_camera_info
+        )
+
+        samples_path = rospkg.RosPack().get_path("zivid_samples")
+        settings_path = samples_path + "/settings/camera_settings.yml"
+        rospy.loginfo("Loading settings from %s", settings_path)
+        self.load_settings_from_file_service = rospy.ServiceProxy(
+            "/zivid_camera/load_settings_from_file", LoadSettingsFromFile
+        )
+        self.load_settings_from_file_service(settings_path)
+
+    def capture(self):
+        rospy.loginfo("Calling capture service")
+        self.capture_service()
+
+    def on_color_camera_info(self, msg: CameraInfo):
+        rospy.loginfo(
+            f"""\
+Received color camera info:
+  width: {msg.width}
+  height: {msg.height}
+  distortion_model: {msg.distortion_model}
+  D: [{msg.D}]
+  K: [{msg.K}]
+  R: [{msg.R}]
+  P: [{msg.P}]
+"""
+        )
+
+    def on_depth_camera_info(self, msg: CameraInfo):
+        rospy.loginfo(
+            f"""\
+Received depth camera info:
+  width: {msg.width}
+  height: {msg.height}
+  distortion_model: {msg.distortion_model}
+  D: [{msg.D}]
+  K: [{msg.K}]
+  R: [{msg.R}]
+  P: [{msg.P}]
+"""
+        )
+
+    def set_intrinsics_source(self, value: str):
+        param_name = (
+            "/zivid_camera/zivid_camera/intrinsics_source"  # Node-private parameter
+        )
+        rospy.loginfo(f"Setting intrinsics source to: {value}")
+
+        rospy.set_param(param_name, value)
+
+        read_value = rospy.get_param(param_name, None)
+        if read_value is not None:
+            if read_value != value:
+                rospy.logwarn(f"Expected '{value}' but got '{read_value}'")
+            else:
+                rospy.loginfo(f"Param set successfully, value: {read_value}")
+        else:
+            rospy.logwarn("Failed to read back param")
+
+
+if __name__ == "__main__":
+    s = Sample()
+    s.set_intrinsics_source("camera")
+    s.capture()
+    s.set_intrinsics_source("frame")
+    s.capture()
+
+    rospy.spin()