diff --git a/README.md b/README.md index 01dc8dc1..3eb4caaf 100644 --- a/README.md +++ b/README.md @@ -220,6 +220,21 @@ Or, if using `roslaunch` specify the parameter using ``: > [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 diff --git a/continuous-integration/run_code_analysis_in_docker.sh b/continuous-integration/run_code_analysis_in_docker.sh index 2d0f0c92..72beb249 100755 --- a/continuous-integration/run_code_analysis_in_docker.sh +++ b/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 diff --git a/zivid_camera/include/zivid_camera.h b/zivid_camera/include/zivid_camera.h index a90c905f..a44d9805 100644 --- a/zivid_camera/include/zivid_camera.h +++ b/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; @@ -93,6 +100,7 @@ class ZividCamera ros::NodeHandle nh_; ros::NodeHandle priv_; std::map color_space_name_value_map_; + std::map intrinsics_source_name_value_map_; ros::Timer camera_connection_keepalive_timer_; CameraStatus camera_status_; bool use_latched_publisher_for_acquisition_done_; diff --git a/zivid_camera/src/zivid_camera.cpp b/zivid_camera/src/zivid_camera.cpp index 52c60389..3b959719 100644 --- a/zivid_camera/src/zivid_camera.cpp +++ b/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("use_latched_publisher_for_points_xyz", use_latched_publisher_for_points_xyz_, false); priv_.param("use_latched_publisher_for_points_xyzrgba", use_latched_publisher_for_points_xyzrgba_, false); priv_.param("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(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 diff --git a/zivid_samples/CMakeLists.txt b/zivid_samples/CMakeLists.txt index b596030a..1d7b4d84 100644 --- a/zivid_samples/CMakeLists.txt +++ b/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 ## diff --git a/zivid_samples/scripts/sample_intrinsics.py b/zivid_samples/scripts/sample_intrinsics.py new file mode 100644 index 00000000..7d3a9a21 --- /dev/null +++ b/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() diff --git a/zivid_samples/src/sample_intrinsics.cpp b/zivid_samples/src/sample_intrinsics.cpp new file mode 100644 index 00000000..f3175475 --- /dev/null +++ b/zivid_samples/src/sample_intrinsics.cpp @@ -0,0 +1,147 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define CHECK(cmd) \ + do \ + { \ + if (!cmd) \ + { \ + throw std::runtime_error{ "\"" #cmd "\" failed!" }; \ + } \ + } while (false) + +namespace +{ +const ros::Duration default_wait_duration{ 30 }; + +void capture() +{ + ROS_INFO("Calling capture service"); + zivid_camera::Capture capture; + CHECK(ros::service::call("/zivid_camera/capture", capture)); +} + +struct ToString +{ + template + std::string operator()(const T& v) const + { + return std::to_string(v); + } +}; + +template +std::string join_list_to_string(const Container& container, ToStringFunc to_string_func) +{ + std::string result; + for (typename Container::const_iterator it = container.begin(); it != container.end(); ++it) + { + if (it != container.begin()) + { + result += ", "; + } + result += to_string_func(*it); + } + return result; +} + +void on_color_camera_info(const sensor_msgs::CameraInfo& msg) +{ + ROS_INFO("Color CameraInfo received"); + ToString to_string; + + ROS_INFO_STREAM("Received color camera info:\n" + << " width: " << msg.width << "\n" + << " height: " << msg.height << "\n" + << " distortion_model: " << msg.distortion_model << "\n" + << " D: [" << join_list_to_string(msg.D, to_string) << "]\n" + << " K: [" << join_list_to_string(msg.K, to_string) << "]\n" + << " R: [" << join_list_to_string(msg.R, to_string) << "]\n" + << " P: [" << join_list_to_string(msg.P, to_string) << "]"); +} + +void on_depth_camera_info(const sensor_msgs::CameraInfo& msg) +{ + ROS_INFO("Depth CameraInfo received"); + ToString to_string; + + ROS_INFO_STREAM("Received depth camera info:\n" + << " width: " << msg.width << "\n" + << " height: " << msg.height << "\n" + << " distortion_model: " << msg.distortion_model << "\n" + << " D: [" << join_list_to_string(msg.D, to_string) << "]\n" + << " K: [" << join_list_to_string(msg.K, to_string) << "]\n" + << " R: [" << join_list_to_string(msg.R, to_string) << "]\n" + << " P: [" << join_list_to_string(msg.P, to_string) << "]"); +} + +void set_intrinsics_source(const std::string& value) +{ + // Use the private namespace of the zivid_camera node + ros::NodeHandle nh("/zivid_camera/zivid_camera"); + + std::string param_name = "intrinsics_source"; // Node-private parameter + ROS_INFO_STREAM("Setting intrinsics source to: " << value); + + nh.setParam(param_name, value); + + std::string read_value; + if (nh.getParam(param_name, read_value)) + { + if (read_value != value) + { + ROS_WARN("Expected '%s' but got '%s'", value.c_str(), read_value.c_str()); + } + else + { + ROS_INFO("Param set successfully, value: %s", read_value.c_str()); + } + } + else + { + ROS_WARN("Failed to read back param"); + } +} + +} // namespace + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "sample_intrinsics_cpp"); + ros::NodeHandle nh; + + ROS_INFO("Starting sample_intrinsics.cpp"); + + CHECK(ros::service::waitForService("/zivid_camera/capture", default_wait_duration)); + auto color_camera_info_subscription = nh.subscribe("/zivid_camera/color/camera_info", 1, on_color_camera_info); + auto depth_camera_info_subscription = nh.subscribe("/zivid_camera/depth/camera_info", 1, on_depth_camera_info); + + ros::AsyncSpinner spinner(1); + spinner.start(); + + 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()); + zivid_camera::LoadSettingsFromFile load_settings_from_file; + load_settings_from_file.request.file_path = settings_path; + CHECK(ros::service::call("/zivid_camera/load_settings_from_file", load_settings_from_file)); + + set_intrinsics_source("camera"); + capture(); + + set_intrinsics_source("frame"); + capture(); + + ros::waitForShutdown(); + + return 0; +} \ No newline at end of file