diff --git a/CMakeLists.txt b/CMakeLists.txt index 71610f9..62eced7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -40,7 +40,9 @@ find_package(camera_calibration_parsers REQUIRED) find_package(OpenCV 4 REQUIRED) add_library(bottlenose_camera_driver SHARED - src/bottlenose_camera_driver.cpp) + src/bottlenose_camera_driver.cpp + src/calibration_parser.cpp + ) target_compile_definitions(bottlenose_camera_driver PRIVATE "COMPOSITION_BUILDING_DLL") target_include_directories(bottlenose_camera_driver diff --git a/README.md b/README.md index 4387b4a..449fef3 100644 --- a/README.md +++ b/README.md @@ -35,57 +35,61 @@ ros2 run image_view image_view --ros-args --remap /image:=/image_raw ### Available parameters -| Parameter | Description | Default | Run-time adjustable | -|-----------------------------------------|-------------------------------------------------------------------------------|-------------------------|---------------------| -| ```mac_address``` | The MAC address of Bottlenose | ```00:00:00:00:00:00``` | :x: | -| ```frame_id``` | The frame_id embedded in image headers | ```camera``` | :heavy_check_mark: | -| ```keep_partial``` | Keep partial images (i.e. corrupted in transmission) | ```false``` | :heavy_check_mark: | -| ```format``` | Format of the camera (*) | ```1920x1440``` | :x: | -| ```fps``` | Target frames per second (*) | ```20``` | :x: | -| ***Image Sensor(s) Controls*** | (**) | | | -| ```exposure``` | Exposure time in milliseconds | ```20``` | :heavy_check_mark: | -| ```gain``` | Analog gain | ```1.0``` | :heavy_check_mark: | -| ```dGainBlue``` | Digital gain for blue pixels in Bayer array | ```1024``` | :heavy_check_mark: | -| ```dgainGB``` | Digital gain for green-blue pixels in Bayer array | ```1024``` | :heavy_check_mark: | -| ```dgainGR``` | Digital gain for green-red pixels in Bayer array | ```1024``` | :heavy_check_mark: | -| ```dGainRed``` | Digital gain for red pixels in Bayer array | ```1024``` | :heavy_check_mark: | -| **Image Processing Controls** | | | | -| ```gamma``` | Gamma correction of the image | ```2.2``` | :heavy_check_mark: | -| ```blackBlue``` | Black level for blue pixels in Bayer array | ```255``` | :heavy_check_mark: | -| ```blackGB``` | Black level for green-blue pixels in Bayer array | ```255``` | :heavy_check_mark: | -| ```blackGR``` | Black level for green-red pixels in Bayer array | ```255``` | :heavy_check_mark: | -| ```blackRed``` | Black level for red pixels in Bayer array | ```255``` | :heavy_check_mark: | -| ```blackGainBlue``` | Black gain for blue pixels in Bayer array | ```0.9375``` | :heavy_check_mark: | -| ```blackGainGB``` | Black gain for green-blue pixels in Bayer array | ```0.9375``` | :heavy_check_mark: | -| ```blackGainGR``` | Black gain for green-red pixels in Bayer array | ```0.9375``` | :heavy_check_mark: | -| ```blackGainRed``` | Black gain for red pixels in Bayer array | ```0.9375``` | :heavy_check_mark: | -| ```brightness``` | Brightness of the image | ```-13107``` | :heavy_check_mark: | -| ```linearContrast``` | Linear contrast of the image | ```136``` | :heavy_check_mark: | -| ```wbBlue``` | White balance for blue component | ```1.0``` | :heavy_check_mark: | -| ```wbGreen``` | White balance for green component | ```1.0``` | :heavy_check_mark: | -| ```wbRed``` | White balance for red component | ```1.0``` | :heavy_check_mark: | -| **Color Correction Controls** | | | | -| ```custom_ccm``` | Enable custom color correction matrix | ```false``` | :x: | -| ```CCMValue00``` | Color correction matrix value at row 0, column 0 (only if custom_ccm=true) | ```1.0``` | :x: | -| ```CCMValue01``` | Color correction matrix value at row 0, column 1 (only if custom_ccm=true) | ```0.0``` | :x: | -| ```CCMValue02``` | Color correction matrix value at row 0, column 2 (only if custom_ccm=true) | ```0.0``` | :x: | -| ```CCMValue10``` | Color correction matrix value at row 1, column 0 (only if custom_ccm=true) | ```0.0``` | :x: | -| ```CCMValue11``` | Color correction matrix value at row 1, column 1 (only if custom_ccm=true) | ```1.0``` | :x: | -| ```CCMValue12``` | Color correction matrix value at row 1, column 2 (only if custom_ccm=true) | ```0.0``` | :x: | -| ```CCMValue20``` | Color correction matrix value at row 2, column 0 (only if custom_ccm=true) | ```0.0``` | :x: | -| ```CCMValue21``` | Color correction matrix value at row 2, column 1 (only if custom_ccm=true) | ```0.0``` | :x: | -| ```CCMValue22``` | Color correction matrix value at row 2, column 2 (only if custom_ccm=true) | ```1.0``` | :x: | -| **GigE Vision Stream Parameters** | | | | -| ```AnswerTimeout``` | Time the GigE Vision Device can take for command response. | ```100``` | :x: | -| ```CommandRetryCount``` | Command attempts before it is considered as failed | ```50``` | :x: | -| ```MaximumPendingResends``` | Maximum number of packets in a block that can be missing | ```0``` | :x: | -| ```MaximumResendRequestRetryByPacket``` | The maximum number of times a resend request can be issued. | ```0``` | :x: | -| ```MaximumResendGroupSize``` | Maximum number of packets to resend at once | ```0``` | :x: | -| ```ResendRequestTimeout``` | Timeout for resend requests in (us) | ```100``` | :x: | -| ```RequestTimeout``` | Maximum time that the data receiver waits for all the packets of a block (ms) | ```10000``` | :x: | -| ```ResetOnIdle``` | Time without packets before resetting itself | ```2000``` | :x: | - -(*) Note: effective limitations are imposed by available bandwidth for the chosen configuration. If the bandwidth is +| Parameter | Description | Default | Run-time adjustable | +|-----------------------------------------|-------------------------------------------------------------------------------|---------------------------|---------------------| +| ```mac_address``` | The MAC address of Bottlenose | ```00:00:00:00:00:00``` | :x: | +| ```frame_id``` | The frame_id embedded in image headers | ```camera``` | :heavy_check_mark: | +| ```keep_partial``` | Keep partial images (i.e. corrupted in transmission) | ```false``` | :heavy_check_mark: | +| ```camera_calibration_file``` | Camera Calibration File (*) | ```config/camera.yaml``` | :x: | +| ```mode``` | Camera output (**) | ```0``` | :x: | +| ```fps``` | Target frames per second (**) | ```20``` | :x: | +| ***Image Sensor(s) Controls*** | (***) | | | +| ```exposure``` | Exposure time in milliseconds | ```20``` | :heavy_check_mark: | +| ```gain``` | Analog gain | ```1.0``` | :heavy_check_mark: | +| ```dGainBlue``` | Digital gain for blue pixels in Bayer array | ```1024``` | :heavy_check_mark: | +| ```dgainGB``` | Digital gain for green-blue pixels in Bayer array | ```1024``` | :heavy_check_mark: | +| ```dgainGR``` | Digital gain for green-red pixels in Bayer array | ```1024``` | :heavy_check_mark: | +| ```dGainRed``` | Digital gain for red pixels in Bayer array | ```1024``` | :heavy_check_mark: | +| **Image Processing Controls** | | | | +| ```gamma``` | Gamma correction of the image | ```2.2``` | :heavy_check_mark: | +| ```blackBlue``` | Black level for blue pixels in Bayer array | ```255``` | :heavy_check_mark: | +| ```blackGB``` | Black level for green-blue pixels in Bayer array | ```255``` | :heavy_check_mark: | +| ```blackGR``` | Black level for green-red pixels in Bayer array | ```255``` | :heavy_check_mark: | +| ```blackRed``` | Black level for red pixels in Bayer array | ```255``` | :heavy_check_mark: | +| ```blackGainBlue``` | Black gain for blue pixels in Bayer array | ```0.9375``` | :heavy_check_mark: | +| ```blackGainGB``` | Black gain for green-blue pixels in Bayer array | ```0.9375``` | :heavy_check_mark: | +| ```blackGainGR``` | Black gain for green-red pixels in Bayer array | ```0.9375``` | :heavy_check_mark: | +| ```blackGainRed``` | Black gain for red pixels in Bayer array | ```0.9375``` | :heavy_check_mark: | +| ```brightness``` | Brightness of the image | ```-13107``` | :heavy_check_mark: | +| ```linearContrast``` | Linear contrast of the image | ```136``` | :heavy_check_mark: | +| ```wbBlue``` | White balance for blue component | ```1.0``` | :heavy_check_mark: | +| ```wbGreen``` | White balance for green component | ```1.0``` | :heavy_check_mark: | +| ```wbRed``` | White balance for red component | ```1.0``` | :heavy_check_mark: | +| **Color Correction Controls** | | | | +| ```custom_ccm``` | Enable custom color correction matrix | ```false``` | :x: | +| ```CCMValue00``` | Color correction matrix value at row 0, column 0 (only if custom_ccm=true) | ```1.0``` | :x: | +| ```CCMValue01``` | Color correction matrix value at row 0, column 1 (only if custom_ccm=true) | ```0.0``` | :x: | +| ```CCMValue02``` | Color correction matrix value at row 0, column 2 (only if custom_ccm=true) | ```0.0``` | :x: | +| ```CCMValue10``` | Color correction matrix value at row 1, column 0 (only if custom_ccm=true) | ```0.0``` | :x: | +| ```CCMValue11``` | Color correction matrix value at row 1, column 1 (only if custom_ccm=true) | ```1.0``` | :x: | +| ```CCMValue12``` | Color correction matrix value at row 1, column 2 (only if custom_ccm=true) | ```0.0``` | :x: | +| ```CCMValue20``` | Color correction matrix value at row 2, column 0 (only if custom_ccm=true) | ```0.0``` | :x: | +| ```CCMValue21``` | Color correction matrix value at row 2, column 1 (only if custom_ccm=true) | ```0.0``` | :x: | +| ```CCMValue22``` | Color correction matrix value at row 2, column 2 (only if custom_ccm=true) | ```1.0``` | :x: | +| **GigE Vision Stream Parameters** | | | | +| ```AnswerTimeout``` | Time the GigE Vision Device can take for command response. | ```100``` | :x: | +| ```CommandRetryCount``` | Command attempts before it is considered as failed | ```50``` | :x: | +| ```MaximumPendingResends``` | Maximum number of packets in a block that can be missing | ```0``` | :x: | +| ```MaximumResendRequestRetryByPacket``` | The maximum number of times a resend request can be issued. | ```0``` | :x: | +| ```MaximumResendGroupSize``` | Maximum number of packets to resend at once | ```0``` | :x: | +| ```ResendRequestTimeout``` | Timeout for resend requests in (us) | ```100``` | :x: | +| ```RequestTimeout``` | Maximum time that the data receiver waits for all the packets of a block (ms) | ```10000``` | :x: | +| ```ResetOnIdle``` | Time without packets before resetting itself | ```2000``` | :x: | + +(*) Note: Required parameter. Even if the image stream is desired to be raw, the calibration dictates the resolution of +the image. + +(**) Note: effective limitations are imposed by available bandwidth for the chosen configuration. If the bandwidth is exceeded the camera will drop frames. (**) Note: For stereo cameras the controls are applied to both sensors simultaneously. @@ -97,10 +101,35 @@ exceeded the camera will drop frames. ``` bottlenose_camera_driver | - +-- camera_image_raw : Color image stream of Bottlenose camera + +-- camera_info : Camera calibration data + +-- image_color : Color image stream of Bottlenose camera (unrectified, if rectification is disabled) + +-- image_color_1 : Color image stream of Bottlenose camera (unrectified, if rectification is disabled), + | stereo only + +-- image_rect_color : Rectified color image stream of Bottlenose camera (if rectification is enabled) + +-- image_rect_color_1 : Rectified color image stream of Bottlenose camera (if rectification is enabled), + | stereo only + +-- depth + +-- image_rect_color : Rectified disparity image stream of Bottlenose camera (if depth is enabled) ``` +In the current release the topics ```image_color```, ```image_rect_color``` and ```depth/image_rect_color``` +are exclusive. See the below for the available configurations: + + * ```mode=0``` ***Color streaming*** + * ```rectify=0``` -> ```image_color{_1}``` (*default*) + * ```rectify=1``` -> ```image_rect_color{_1}``` + * ***Stereo only*** + * Set sensor to ```0``` or ```1``` to switch between left or right sensor + + * ```mode=1``` ***Stereo streaming*** + * ```rectify=0``` -> ```image_color``` and ```image_color_1``` + * ```rectify=1``` -> ```image_rect_color``` and ```image_rect_color_1``` + + * ```mode=3``` ***Disparity streaming*** + * ```rectify=1``` -> ```depth/image_rect_color``` + * Note ```rectify=0``` is not supported for depth streaming ## References * Bottlenose [Getting Started](https://docs.labforge.ca/docs/getting-started) + * Bottlenose [Calibration Guide](https://docs.labforge.ca/docs/3d-modules) * Bottlenose [SDK Demos](https://github.com/labforge/sdk-demos) diff --git a/config/camera.yaml b/config/camera.yaml index eebff7e..eb0bfda 100755 --- a/config/camera.yaml +++ b/config/camera.yaml @@ -1,6 +1,6 @@ -image_width: 640 -image_height: 480 -camera_name: camera +image_width: 1920 +image_height: 1440 +camera_name: Bottlenose camera_matrix: rows: 3 cols: 3 diff --git a/include/bottlenose_camera_driver.hpp b/include/bottlenose_camera_driver.hpp index 9717809..ea078d4 100644 --- a/include/bottlenose_camera_driver.hpp +++ b/include/bottlenose_camera_driver.hpp @@ -85,8 +85,16 @@ namespace bottlenose_camera_driver { std::thread m_management_thread; ///< Management thread handle. std::shared_ptr m_cinfo_manager; - /// Camera publisher. - image_transport::CameraPublisher m_camera_pub; + /// Camera publisher: color image (unrectified). + image_transport::CameraPublisher m_image_color; + /// Camera publisher: color image (rectified). + image_transport::CameraPublisher m_image_rect_color; + /// Camera publisher: color image, sensor 1 (unrectified). + image_transport::CameraPublisher m_image_color_1; + /// Camera publisher: color image, sensor 1 (rectified). + image_transport::CameraPublisher m_image_rect_color_1; + /// Camera publisher: depth stream. + image_transport::CameraPublisher m_depth_image_rect; }; } // namespace bottlenose_camera_driver #endif //__BOTTLENOSE_CAMERA_DRIVER_HPP__ diff --git a/include/bottlenose_parameters.hpp b/include/bottlenose_parameters.hpp index db36451..f521526 100644 --- a/include/bottlenose_parameters.hpp +++ b/include/bottlenose_parameters.hpp @@ -27,11 +27,19 @@ typedef struct { const rclcpp::ParameterValue & default_value; } parameter_t; +typedef enum { + MODE_MONO, + MODE_STEREO, + MODE_DEPTH, + /* MODE_RGBD: FIXME: as soon as the FW update rolls in for this */ +} camera_mode_t; + const parameter_t bottlenose_parameters[] = { {"frame_id", rclcpp::ParameterValue("camera")}, - {"format", rclcpp::ParameterValue("1920x1440")}, + {"camera_calibration_file", rclcpp::ParameterValue("package://bottlenose_camera_driver/config/camera.yaml")}, {"mac_address", rclcpp::ParameterValue("00:00:00:00:00:00")}, {"keep_partial", rclcpp::ParameterValue(false)}, + {"mode", rclcpp::ParameterValue(0)}, /* Device parameters */ {"fps", rclcpp::ParameterValue(20.0)}, {"exposure", rclcpp::ParameterValue(20.0)}, diff --git a/include/calibration_parser.hpp b/include/calibration_parser.hpp new file mode 100644 index 0000000..b12b309 --- /dev/null +++ b/include/calibration_parser.hpp @@ -0,0 +1,19 @@ +/****************************************************************************** + * Copyright 2023 Labforge Inc. * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); * + * you may not use this project except in compliance with the License. * + * You may obtain a copy of the License at * + * * + * http://www.apache.org/licenses/LICENSE-2.0 * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + ****************************************************************************** + +@file calibration_parser.hpp Calibration Parser for Bottlenose +@author Thomas Reidemeister +*/ \ No newline at end of file diff --git a/src/bottlenose_camera_driver.cpp b/src/bottlenose_camera_driver.cpp index dd0a1aa..cfaa129 100644 --- a/src/bottlenose_camera_driver.cpp +++ b/src/bottlenose_camera_driver.cpp @@ -67,14 +67,14 @@ CameraDriver::CameraDriver(const rclcpp::NodeOptions &node_options) : Node("bott } rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_default; - m_camera_pub = image_transport::create_camera_publisher(this, "image_raw", custom_qos_profile); + m_image_color = image_transport::create_camera_publisher(this, "image_color", custom_qos_profile); + m_image_rect_color = image_transport::create_camera_publisher(this, "m_image_rect_color", custom_qos_profile); + m_image_color_1 = image_transport::create_camera_publisher(this, "image_color_1", custom_qos_profile); + m_image_rect_color_1 = image_transport::create_camera_publisher(this, "m_image_rect_color_1", custom_qos_profile); + m_depth_image_rect = image_transport::create_camera_publisher(this, "depth/image_rect_color", custom_qos_profile); m_cinfo_manager = std::make_shared(this); -// /* get ROS2 config parameter for camera calibration file */ -// auto camera_calibration_file_param_ = this->declare_parameter("camera_calibration_file", "file://config/camera.yaml"); -// m_cinfo_manager->loadCameraInfo(camera_calibration_file_param_); - m_timer = this->create_wall_timer(1ms, std::bind(&CameraDriver::status_callback, this)); } @@ -105,11 +105,16 @@ void CameraDriver::status_callback() { m_management_thread.join(); } } - auto mac_address = this->get_parameter("mac_address").as_string(); + auto mac_address = get_parameter("mac_address").as_string(); if(mac_address == "00:00:00:00:00:00") { RCLCPP_INFO_ONCE(get_logger(), "Bottlenose undefined please set mac_address"); return; } + string calibration_file = get_parameter("camera_calibration_file").as_string(); + if(!m_cinfo_manager->loadCameraInfo(calibration_file)) { + RCLCPP_ERROR(get_logger(), "Failed to load camera calibration file %s", calibration_file.c_str()); + return; + } m_mac_address = mac_address; done = false; m_management_thread = std::thread(&CameraDriver::management_thread, this); @@ -149,7 +154,7 @@ std::shared_ptr CameraDriver::convertFrameToMessage(PvB uint64_t nanoseconds = (image->GetTimestamp() - seconds * 1e6) * 1e3; //nanoseconds ros_image.header.stamp.nanosec = nanoseconds; ros_image.header.stamp.sec = seconds; - ros_image.header.frame_id = this->get_parameter("frame_id").as_string(); + ros_image.header.frame_id = get_parameter("frame_id").as_string(); auto msg_ptr_ = std::make_shared(ros_image); return msg_ptr_; @@ -283,23 +288,10 @@ bool CameraDriver::set_format() { RCLCPP_ERROR(get_logger(), "Could not configure format"); return false; } - // Find the position of 'x' in the format specification string - auto format = get_parameter("format").as_string(); - auto xPos = format.find('x'); - - // Extract the width and height substrings - std::string widthStr = format.substr(0, xPos); - std::string heightStr = format.substr(xPos + 1); - - // Convert the width and height strings to integers - int width = std::stoi(widthStr); - int height = std::stoi(heightStr); - - // Print the width and height - RCLCPP_DEBUG_STREAM(get_logger(), "Decoded format " << width << " x " << height); - PvResult res = heightParam->SetValue(height); + m_cinfo_manager->getCameraInfo().height; + PvResult res = heightParam->SetValue(m_cinfo_manager->getCameraInfo().height); if(res.IsFailure()) { - RCLCPP_ERROR_STREAM(get_logger(), "Could not configure format to " << format); + RCLCPP_ERROR(get_logger(), "Could not configure format, check your camera configuration"); return false; } // Wait for parameter pass-through @@ -308,14 +300,17 @@ bool CameraDriver::set_format() { int64_t width_in; res = widthParam->GetValue(width_in); if(res.IsFailure()) { - RCLCPP_ERROR_STREAM(get_logger(), "Could not configure format to " << format); + RCLCPP_ERROR_STREAM(get_logger(), "Could not configure format to " << + m_cinfo_manager->getCameraInfo().width << " x " << m_cinfo_manager->getCameraInfo().height); return false; } - if(width_in != width) { - RCLCPP_ERROR_STREAM(get_logger(), "Could not configure format to " << format << " actual format is " << width_in << " x " << height); + if(width_in != m_cinfo_manager->getCameraInfo().width) { + RCLCPP_ERROR_STREAM(get_logger(), "Could not configure format to " << + m_cinfo_manager->getCameraInfo().width << " x " << m_cinfo_manager->getCameraInfo().height); return false; } - RCLCPP_DEBUG_STREAM(get_logger(), "Configured format to " << format); + RCLCPP_DEBUG_STREAM(get_logger(), "Configured format to " << + m_cinfo_manager->getCameraInfo().width << " x " << m_cinfo_manager->getCameraInfo().height); return true; } @@ -496,7 +491,7 @@ void CameraDriver::management_thread() { sensor_msgs::msg::CameraInfo::SharedPtr info_msg( new sensor_msgs::msg::CameraInfo(m_cinfo_manager->getCameraInfo())); info_msg->header = m_image_msg->header; - m_camera_pub.publish(m_image_msg, info_msg); + m_image_color.publish(m_image_msg, info_msg); } break; diff --git a/src/calibration_parser.cpp b/src/calibration_parser.cpp new file mode 100644 index 0000000..f5e162c --- /dev/null +++ b/src/calibration_parser.cpp @@ -0,0 +1,102 @@ +/****************************************************************************** + * Copyright 2023 Labforge Inc. * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); * + * you may not use this project except in compliance with the License. * + * You may obtain a copy of the License at * + * * + * http://www.apache.org/licenses/LICENSE-2.0 * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + ****************************************************************************** + +@file calibration_parser.cpp Implementation of Calibration Parser for Bottlenose +@author Thomas Reidemeister +*/ +#include +#include +#include +#include + +std::unordered_map LoadFlatYamlParameters(const std::string &fname = "", int sensors = 0) { + std::unordered_map kdata; + + if (fname.empty() || (fname.length() >= 5 && + (fname.substr(fname.length() - 5) == ".yaml" || fname.substr(fname.length() - 4) == ".yml"))) { + return kdata; + } + + if (sensors == 0) { + return kdata; + } + + try { + YAML::Node calib = YAML::LoadFile(fname); + + int nCameras = calib.size(); + if (nCameras != sensors) { + return kdata; + } + + int tvec_count = 0; + int rvec_count = 0; + + for (const auto &cam: calib) { + std::string camKey = cam.first.as(); + + if (camKey != "cam0" && camKey != "cam1") { + kdata.clear(); + return kdata; + } + + std::string id = camKey.substr(camKey.length() - 1); + + kdata["fx" + id] = cam.second["fx"].as(); + kdata["fy" + id] = cam.second["fy"].as(); + kdata["cx" + id] = cam.second["cx"].as(); + kdata["cy" + id] = cam.second["cy"].as(); + + kdata["k1" + id] = cam.second["k1"].as(); + kdata["k2" + id] = cam.second["k2"] ? cam.second["k2"].as() : 0.0; + kdata["k3" + id] = cam.second["k3"] ? cam.second["k3"].as() : 0.0; + kdata["p1" + id] = cam.second["p1"] ? cam.second["p1"].as() : 0.0; + kdata["p2" + id] = cam.second["p2"] ? cam.second["p2"].as() : 0.0; + + std::vector tvec = {0.0, 0.0, 0.0}; + if (cam.second["tvec"]) { + tvec = cam.second["tvec"].as>(); + tvec_count++; + } + + std::vector rvec = {0.0, 0.0, 0.0}; + if (cam.second["rvec"]) { + rvec = cam.second["rvec"].as>(); + rvec_count++; + } + + kdata["tx" + id] = tvec[0]; + kdata["ty" + id] = tvec[1]; + kdata["tz" + id] = tvec[2]; + kdata["rx" + id] = rvec[0]; + kdata["ry" + id] = rvec[1]; + kdata["rz" + id] = rvec[2]; + + kdata["kWidth"] = cam.second["width"].as(); + kdata["kHeight"] = cam.second["height"].as(); + } + + if (tvec_count < (sensors - 1) || rvec_count < (sensors - 1)) { + kdata.clear(); + return kdata; + } + } catch (const std::exception &e) { + kdata.clear(); + return kdata; + } + + return kdata; +}