diff --git a/doc/calibration_file_format.md b/doc/calibration_file_format.md index 31a38ba..a891895 100644 --- a/doc/calibration_file_format.md +++ b/doc/calibration_file_format.md @@ -16,6 +16,17 @@ cameras: type: pinhole_radtan intrinsics: [1057.79, 1059.8, 962.78, 581.29] distortion_coeffs: [-0.149116, 0.09615, -0.000526577, -0.000567049, -0.022971] +imus: + imu1: + frame_id: imu1 + accel_matrix: [1, 0, 0, 0, 1, 0, 0, 0, 1] + accel_offset: [0, 0, 0] + accel_noise_density: [1.86e-03, 1.86e-03, 1.86e-03] + accel_random_walk: [4.33e-04, 4.33e-04, 4.33e-04] + gyro_matrix: [1, 0, 0, 0, 1, 0, 0, 0, 1] + gyro_offset: [0, 0, 0] + gyro_noise_density: [1.87e-04, 1.87e-04, 1.87e-04] + gyro_random_walk: [2.66e-05, 2.66e-05, 2.66e-05] transforms: transform1: frame_id: camera1 @@ -29,24 +40,12 @@ transforms: rotation: [0.0 ,0.0 ,0.0 ,1.0] # x,y,z,w ``` -## 传感器外参说明 - -传感器的外参包括以下四个量 - -* frame_id：传感器的frame id -* child_frame_id：另一个传感器的frame id -* translation：外参坐标变换中的平移部分 -* rotation：外参坐标变换中的旋转部分，采用四元数表示，顺序为`[x,y,z,w]` - -这里标定出的坐标变换表示为 $T_\text{frame id - child frame id}$，等价以下两种说法： - -* 在`frame_id`传感器坐标系下，`child_frame_id`的位姿。 -* `child_frame_id`坐标系到`frame_id`坐标系下的位姿变换。 - ## 相机内参说明 相机内参包括以下三个量 +* frame_id：传感器的frame id +* height，width：标定相机的图片分辨率 * type：采用的相机模型类型，为一个字符串 * intrinsics：相机内参系数，为一个float数组 * distortion_coeffs：畸变参数系数，为一个float数组 @@ -75,3 +74,39 @@ transforms: * distortion_coeffs: $[k_1, k_2, p_1, p_2, k_3]$ ，其中k为径向畸变参数，p为切向畸变参数。 * 参考：https://docs.opencv.org/4.x/dd/d12/tutorial_omnidir_calib_main.html + +## Imu内参说明 + +IMU内参包括加速度计accel和陀螺仪gyro两组数据 + +* frame_id：传感器的frame id +* accel_matrix，accel_offset：速度计accel的仿射变换模型参数，修正测量值。 +* accel_noise_density，accel_random_walk：速度计accel的白噪声以及零偏不稳定（随机游走）噪声。 +* gyro_matrix，gyro_offset：陀螺仪gyro的仿射变换模型参数，修正测量值。 +* gyro_noise_density，gyro_random_walk：陀螺仪gyro的白噪声以及零偏不稳定（随机游走）噪声。 + +速度计accel和陀螺仪gyro采用仿射变换建立确定性误差模型，修正scale,misalignment等误差，测量值修正公式如下： +$$ +\hat x_{accel} = A_{accel}x_{accel} + b_{accel} \\ +\hat x_{gyro} = A_{gyro}x_{gyro} + b_{gyro} \\ +$$ + +* 其中$x$是原始测量值，$\hat x$是标定修正后的值，A对应matrix参数，b对应offset参数 + +random_walk和noise_density是随机误差（不确定性误差）模型参数，一般使用allan方差分析得到 +* 参考：https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model + + +## 传感器外参说明 + +传感器的外参包括以下四个量 + +* frame_id：传感器的frame id +* child_frame_id：另一个传感器的frame id +* translation：外参坐标变换中的平移部分 +* rotation：外参坐标变换中的旋转部分，采用四元数表示，顺序为`[x,y,z,w]` + +这里标定出的坐标变换表示为 $T_\text{frame id - child frame id}$，等价以下两种说法： + +* 在`frame_id`传感器坐标系下，`child_frame_id`的位姿。 +* `child_frame_id`坐标系到`frame_id`坐标系下的位姿变换。 diff --git a/ssct_camera_intrinsic_calib/include/ssct_camera_intrinsic_calib/calibration_node.hpp b/ssct_camera_intrinsic_calib/include/ssct_camera_intrinsic_calib/calibration_node.hpp index ba6b8a1..e02c2d2 100644 --- a/ssct_camera_intrinsic_calib/include/ssct_camera_intrinsic_calib/calibration_node.hpp +++ b/ssct_camera_intrinsic_calib/include/ssct_camera_intrinsic_calib/calibration_node.hpp @@ -22,7 +22,7 @@ #include "rclcpp/rclcpp.hpp" #include "ssct_common/publisher/image_publisher.hpp" #include "ssct_common/subscriber/image_subscriber.hpp" -#include "ssct_common/calibration_params.hpp" +#include "ssct_common/calib_param_manager.hpp" #include "ssct_camera_intrinsic_calib/pinhole_calibrator.hpp" #include "ssct_interfaces/msg/calibration_status.hpp" #include "ssct_interfaces/msg/calibration_command.hpp" @@ -72,7 +72,7 @@ class CalibrationNode bool exit_{false}; // data std::deque sensor_data_buffer_; - std::shared_ptr calibration_params_; + std::shared_ptr calib_param_manager_; uint8_t state_; bool success_{false}; bool is_auto_mode_{true}; diff --git a/ssct_camera_intrinsic_calib/src/calibration_node.cpp b/ssct_camera_intrinsic_calib/src/calibration_node.cpp index 6022efa..7cee808 100644 --- a/ssct_camera_intrinsic_calib/src/calibration_node.cpp +++ b/ssct_camera_intrinsic_calib/src/calibration_node.cpp @@ -63,7 +63,7 @@ CalibrationNode::CalibrationNode(const rclcpp::NodeOptions & options) // calibrator YAML::Node config_node = YAML::LoadFile(calibrator_config); calibrator_ = std::make_shared(config_node["pinhole_calibrator"]); - calibration_params_ = std::make_shared(); + calib_param_manager_ = std::make_shared(); // initialize status status_msg_.frame_id = frame_id_; status_msg_.sensor_topic = image_sub_->get_topic_name(); @@ -160,10 +160,10 @@ void CalibrationNode::save_result() return; } if (std::filesystem::exists(output_file_)) { - if (!calibration_params_->load(output_file_)) { + if (!calib_param_manager_->load(output_file_)) { RCLCPP_FATAL( node_->get_logger(), "failed to load existed calibration data, %s", - calibration_params_->error_message().c_str()); + calib_param_manager_->error_message().c_str()); return; } } @@ -175,8 +175,8 @@ void CalibrationNode::save_result() param.width = image_size.width; param.intrinsics = calibrator_->get_intrinsics(); param.distortion_coeffs = calibrator_->get_distortion_coeffs(); - calibration_params_->add_camera_intrinsic_param(frame_id_, param); - if (calibration_params_->save(output_file_)) { + calib_param_manager_->add_camera_intrinsic_param(param); + if (calib_param_manager_->save(output_file_)) { RCLCPP_INFO(node_->get_logger(), "successed to save result: %s", output_file_.c_str()); } else { RCLCPP_INFO(node_->get_logger(), "failed to save result: %s", output_file_.c_str()); diff --git a/ssct_common/CMakeLists.txt b/ssct_common/CMakeLists.txt index 7e28c38..f2d62b6 100644 --- a/ssct_common/CMakeLists.txt +++ b/ssct_common/CMakeLists.txt @@ -50,7 +50,7 @@ add_library(${PROJECT_NAME} SHARED src/dummy_sensor/dummy_lidar_node.cpp src/dummy_sensor/dummy_imu_node.cpp # utils - src/calibration_params.cpp + src/calib_param_manager.cpp src/msg_utils.cpp src/tic_toc.cpp ) diff --git a/ssct_common/include/ssct_common/calib_param/camera_intrinsic_param.hpp b/ssct_common/include/ssct_common/calib_param/camera_intrinsic_param.hpp new file mode 100644 index 0000000..793420e --- /dev/null +++ b/ssct_common/include/ssct_common/calib_param/camera_intrinsic_param.hpp @@ -0,0 +1,35 @@ +// Copyright 2025 Gezp (https://github.com/gezp). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file 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. + +#pragma once + +#include + +#include +#include + +namespace ssct_common +{ + +struct CameraIntrinsicParam +{ + std::string frame_id; + int height; + int width; + std::string type; + std::vector intrinsics; + std::vector distortion_coeffs; +}; + +} // namespace ssct_common diff --git a/ssct_common/include/ssct_common/calib_param/extrinsic_param.hpp b/ssct_common/include/ssct_common/calib_param/extrinsic_param.hpp new file mode 100644 index 0000000..5e61ffd --- /dev/null +++ b/ssct_common/include/ssct_common/calib_param/extrinsic_param.hpp @@ -0,0 +1,31 @@ +// Copyright 2025 Gezp (https://github.com/gezp). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file 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. + +#pragma once + +#include + +#include + +namespace ssct_common +{ + +struct ExtrinsicParam +{ + std::string frame_id; + std::string child_frame_id; + Eigen::Matrix4d transform; +}; + +} // namespace ssct_common diff --git a/ssct_common/include/ssct_common/calib_param/imu_intrinsic_param.hpp b/ssct_common/include/ssct_common/calib_param/imu_intrinsic_param.hpp new file mode 100644 index 0000000..b1ac7c5 --- /dev/null +++ b/ssct_common/include/ssct_common/calib_param/imu_intrinsic_param.hpp @@ -0,0 +1,40 @@ +// Copyright 2025 Gezp (https://github.com/gezp). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file 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. + +#pragma once + +#include + +#include +#include + +namespace ssct_common +{ + +struct ImuIntrinsicParam +{ + std::string frame_id; + // for accelerometer + Eigen::Matrix3d accel_matrix; + Eigen::Vector3d accel_offset; + Eigen::Vector3d accel_noise_density; + Eigen::Vector3d accel_random_walk; + // for gyroscope + Eigen::Matrix3d gyro_matrix; + Eigen::Vector3d gyro_offset; + Eigen::Vector3d gyro_noise_density; + Eigen::Vector3d gyro_random_walk; +}; + +} // namespace ssct_common diff --git a/ssct_common/include/ssct_common/calibration_params.hpp b/ssct_common/include/ssct_common/calib_param_manager.hpp similarity index 68% rename from ssct_common/include/ssct_common/calibration_params.hpp rename to ssct_common/include/ssct_common/calib_param_manager.hpp index 7fda3f4..7d13610 100644 --- a/ssct_common/include/ssct_common/calibration_params.hpp +++ b/ssct_common/include/ssct_common/calib_param_manager.hpp @@ -21,41 +21,33 @@ #include #include -namespace ssct_common -{ +#include "ssct_common/calib_param/camera_intrinsic_param.hpp" +#include "ssct_common/calib_param/imu_intrinsic_param.hpp" +#include "ssct_common/calib_param/extrinsic_param.hpp" -struct CameraIntrinsicParam -{ - std::string frame_id; - int height; - int width; - std::string type; - std::vector intrinsics; - std::vector distortion_coeffs; -}; - -struct ExtrinsicParam +namespace ssct_common { - std::string frame_id; - std::string child_frame_id; - Eigen::Matrix4d transform; -}; -class CalibrationParams +class CalibParamManager { public: - CalibrationParams() = default; - ~CalibrationParams() = default; + CalibParamManager() = default; + ~CalibParamManager() = default; // for camera intrinsic params - bool add_camera_intrinsic_param(const std::string & frame_id, const CameraIntrinsicParam & param); + bool add_camera_intrinsic_param(const CameraIntrinsicParam & param); bool get_camera_intrinsic_param(const std::string & frame_id, CameraIntrinsicParam & param); void remove_camera_intrinsic_param(const std::string & frame_id); + // for imu intrinsic params + bool add_imu_intrinsic_param(const ImuIntrinsicParam & param); + bool get_imu_intrinsic_param(const std::string & frame_id, ImuIntrinsicParam & param); + void remove_imu_intrinsic_param(const std::string & frame_id); // for extrinsic params + bool add_extrinsic_param(const ExtrinsicParam & param); bool add_extrinsic_param( const std::string & frame_id, const std::string & child_frame_id, const Eigen::Matrix4d & transform); bool get_extrinsic_param( - const std::string & frame_id, const std::string & child_frame_id, Eigen::Matrix4d & transform); + const std::string & frame_id, const std::string & child_frame_id, ExtrinsicParam & param); void remove_extrinsic_param(const std::string & frame_id, const std::string & child_frame_id); // save & load bool save(const std::string & file); @@ -65,6 +57,7 @@ class CalibrationParams private: std::map camera_intrinsic_params_; + std::map imu_intrinsic_params_; std::map extrinsic_params_; std::string error_message_; }; diff --git a/ssct_common/src/calibration_params.cpp b/ssct_common/src/calib_param_manager.cpp similarity index 56% rename from ssct_common/src/calibration_params.cpp rename to ssct_common/src/calib_param_manager.cpp index 9e6e60d..df11677 100644 --- a/ssct_common/src/calibration_params.cpp +++ b/ssct_common/src/calib_param_manager.cpp @@ -17,7 +17,7 @@ #include #include -#include "ssct_common/calibration_params.hpp" +#include "ssct_common/calib_param_manager.hpp" namespace ssct_common { @@ -36,14 +36,37 @@ std::string to_string(const std::vector & data) return ss.str(); } -bool CalibrationParams::add_camera_intrinsic_param( - const std::string & frame_id, const CameraIntrinsicParam & param) +std::string to_string(const Eigen::Matrix3d & data) { - camera_intrinsic_params_[frame_id] = param; + std::vector vec(data.data(), data.data() + data.size()); + return to_string(vec); +} + +std::string to_string(const Eigen::Vector3d & data) +{ + std::vector vec(data.data(), data.data() + data.size()); + return to_string(vec); +} + +Eigen::Matrix3d to_Matrix3d(const std::vector & data) +{ + assert(data.size() == 9); + return Eigen::Map(data.data()); +} + +Eigen::Vector3d to_Vector3d(const std::vector & data) +{ + assert(data.size() == 3); + return Eigen::Map(data.data()); +} + +bool CalibParamManager::add_camera_intrinsic_param(const CameraIntrinsicParam & param) +{ + camera_intrinsic_params_[param.frame_id] = param; return true; } -bool CalibrationParams::get_camera_intrinsic_param( +bool CalibParamManager::get_camera_intrinsic_param( const std::string & frame_id, CameraIntrinsicParam & param) { auto it = camera_intrinsic_params_.find(frame_id); @@ -54,7 +77,7 @@ bool CalibrationParams::get_camera_intrinsic_param( return true; } -void CalibrationParams::remove_camera_intrinsic_param(const std::string & frame_id) +void CalibParamManager::remove_camera_intrinsic_param(const std::string & frame_id) { auto it = camera_intrinsic_params_.find(frame_id); if (it != camera_intrinsic_params_.end()) { @@ -62,7 +85,39 @@ void CalibrationParams::remove_camera_intrinsic_param(const std::string & frame_ } } -bool CalibrationParams::add_extrinsic_param( +bool CalibParamManager::add_imu_intrinsic_param(const ImuIntrinsicParam & param) +{ + imu_intrinsic_params_[param.frame_id] = param; + return true; +} + +bool CalibParamManager::get_imu_intrinsic_param( + const std::string & frame_id, ImuIntrinsicParam & param) +{ + auto it = imu_intrinsic_params_.find(frame_id); + if (it == imu_intrinsic_params_.end()) { + return false; + } + param = it->second; + return true; +} + +void CalibParamManager::remove_imu_intrinsic_param(const std::string & frame_id) +{ + auto it = imu_intrinsic_params_.find(frame_id); + if (it != imu_intrinsic_params_.end()) { + imu_intrinsic_params_.erase(it); + } +} + +bool CalibParamManager::add_extrinsic_param(const ExtrinsicParam & param) +{ + std::string key = param.frame_id + "_tf_" + param.child_frame_id; + extrinsic_params_[key] = param; + return true; +} + +bool CalibParamManager::add_extrinsic_param( const std::string & frame_id, const std::string & child_frame_id, const Eigen::Matrix4d & transform) { @@ -70,24 +125,22 @@ bool CalibrationParams::add_extrinsic_param( param.frame_id = frame_id; param.child_frame_id = child_frame_id; param.transform = transform; - std::string key = frame_id + "_tf_" + child_frame_id; - extrinsic_params_[key] = param; - return true; + return add_extrinsic_param(param); } -bool CalibrationParams::get_extrinsic_param( - const std::string & frame_id, const std::string & child_frame_id, Eigen::Matrix4d & transform) +bool CalibParamManager::get_extrinsic_param( + const std::string & frame_id, const std::string & child_frame_id, ExtrinsicParam & param) { std::string key = frame_id + "_tf_" + child_frame_id; auto it = extrinsic_params_.find(key); if (it == extrinsic_params_.end()) { return false; } - transform = it->second.transform; + param = it->second; return true; } -void CalibrationParams::remove_extrinsic_param( +void CalibParamManager::remove_extrinsic_param( const std::string & frame_id, const std::string & child_frame_id) { auto key = frame_id + "_tf_" + child_frame_id; @@ -97,7 +150,7 @@ void CalibrationParams::remove_extrinsic_param( } } -bool CalibrationParams::save(const std::string & file) +bool CalibParamManager::save(const std::string & file) { std::ofstream ofs; ofs.open(file); @@ -120,6 +173,23 @@ bool CalibrationParams::save(const std::string & file) cnt++; } } + if (!imu_intrinsic_params_.empty()) { + ofs << "imus:" << std::endl; + int cnt = 1; + for (auto & [key, param] : imu_intrinsic_params_) { + ofs << " imu" << cnt << ":" << std::endl; + ofs << " frame_id: " << param.frame_id << std::endl; + ofs << " accel_matrix: " << to_string(param.accel_matrix) << std::endl; + ofs << " accel_offset: " << to_string(param.accel_offset) << std::endl; + ofs << " accel_noise_density: " << to_string(param.accel_noise_density) << std::endl; + ofs << " accel_random_walk: " << to_string(param.accel_random_walk) << std::endl; + ofs << " gyro_matrix: " << to_string(param.gyro_matrix) << std::endl; + ofs << " gyro_offset: " << to_string(param.gyro_offset) << std::endl; + ofs << " gyro_noise_density: " << to_string(param.gyro_noise_density) << std::endl; + ofs << " gyro_random_walk: " << to_string(param.gyro_random_walk) << std::endl; + cnt++; + } + } if (!extrinsic_params_.empty()) { ofs << "transforms:" << std::endl; int cnt = 1; @@ -140,7 +210,7 @@ bool CalibrationParams::save(const std::string & file) return true; } -bool CalibrationParams::load(const std::string & file) +bool CalibParamManager::load(const std::string & file) { camera_intrinsic_params_.clear(); extrinsic_params_.clear(); @@ -158,13 +228,40 @@ bool CalibrationParams::load(const std::string & file) param.width = camera["width"].as(); param.intrinsics = camera["intrinsics"].as>(); param.distortion_coeffs = camera["distortion_coeffs"].as>(); - add_camera_intrinsic_param(param.frame_id, param); + add_camera_intrinsic_param(param); } catch (std::exception & e) { error_message_ = std::string("invalid camera data [") + key + "]"; return false; } } } + auto imus = config["imus"]; + if (imus.IsDefined() && imus.IsMap()) { + for (auto it = cameras.begin(); it != imus.end(); ++it) { + std::string key = "unknown"; + try { + key = it->first.as(); + YAML::Node camera = it->second; + ImuIntrinsicParam param; + param.frame_id = camera["frame_id"].as(); + param.accel_matrix = to_Matrix3d(camera["accel_matrix"].as>()); + param.accel_offset = to_Vector3d(camera["accel_offset"].as>()); + param.accel_noise_density = + to_Vector3d(camera["accel_noise_density"].as>()); + param.accel_random_walk = + to_Vector3d(camera["accel_random_walk"].as>()); + param.gyro_matrix = to_Matrix3d(camera["gyro_matrix"].as>()); + param.gyro_offset = to_Vector3d(camera["gyro_offset"].as>()); + param.gyro_noise_density = + to_Vector3d(camera["gyro_noise_density"].as>()); + param.gyro_random_walk = to_Vector3d(camera["gyro_random_walk"].as>()); + add_imu_intrinsic_param(param); + } catch (std::exception & e) { + error_message_ = std::string("invalid imu data [") + key + "]"; + return false; + } + } + } auto transforms = config["transforms"]; if (transforms.IsDefined() && transforms.IsMap()) { for (auto it = transforms.begin(); it != transforms.end(); ++it) { @@ -189,7 +286,7 @@ bool CalibrationParams::load(const std::string & file) return true; } -std::string CalibrationParams::error_message() +std::string CalibParamManager::error_message() { return error_message_; } diff --git a/ssct_lidar_cam_manual_calib/include/ssct_lidar_cam_manual_calib/calibration_node.hpp b/ssct_lidar_cam_manual_calib/include/ssct_lidar_cam_manual_calib/calibration_node.hpp index 9817017..eb254ab 100644 --- a/ssct_lidar_cam_manual_calib/include/ssct_lidar_cam_manual_calib/calibration_node.hpp +++ b/ssct_lidar_cam_manual_calib/include/ssct_lidar_cam_manual_calib/calibration_node.hpp @@ -25,7 +25,7 @@ #include "ssct_common/publisher/image_publisher.hpp" #include "ssct_common/subscriber/image_subscriber.hpp" #include "ssct_common/subscriber/cloud_subscriber.hpp" -#include "ssct_common/calibration_params.hpp" +#include "ssct_common/calib_param_manager.hpp" #include "ssct_interfaces/msg/calibration_status.hpp" #include "ssct_interfaces/msg/calibration_command.hpp" #include "ssct_lidar_cam_manual_calib/lidar_projector.hpp" @@ -55,7 +55,7 @@ class CalibrationNode bool read_data(); void clear_data(); void process_command(const ssct_interfaces::msg::CalibrationCommand & msg); - bool update_calibration_params(const std::string & key, float value); + bool update_calib_param_manager(const std::string & key, float value); void update_status_msg(uint8_t state, const std::string & info, uint8_t progress); void save_result(); bool run(); diff --git a/ssct_lidar_cam_manual_calib/src/calibration_node.cpp b/ssct_lidar_cam_manual_calib/src/calibration_node.cpp index 6302621..6daf3ff 100644 --- a/ssct_lidar_cam_manual_calib/src/calibration_node.cpp +++ b/ssct_lidar_cam_manual_calib/src/calibration_node.cpp @@ -49,11 +49,13 @@ CalibrationNode::CalibrationNode(const rclcpp::NodeOptions & options) return; } // read calibration data - auto calibration_params = std::make_shared(); - calibration_params->load(initial_calibration_file_); - calibration_params->get_extrinsic_param(camera_frame_id_, lidar_frame_id_, T_camera_lidar_); + auto calib_param_manager = std::make_shared(); + calib_param_manager->load(initial_calibration_file_); + ssct_common::ExtrinsicParam extrinsic_param; + calib_param_manager->get_extrinsic_param(camera_frame_id_, lidar_frame_id_, extrinsic_param); + T_camera_lidar_ = extrinsic_param.transform; ssct_common::CameraIntrinsicParam param; - calibration_params->get_camera_intrinsic_param(camera_frame_id_, param); + calib_param_manager->get_camera_intrinsic_param(camera_frame_id_, param); if (param.intrinsics.size() != 4) { RCLCPP_FATAL(node_->get_logger(), "camera intrinsic in calibration file is invalid"); return; @@ -196,7 +198,7 @@ void CalibrationNode::clear_data() current_sensor_data_.time = -1; } -bool CalibrationNode::update_calibration_params(const std::string & key, float value) +bool CalibrationNode::update_calib_param_manager(const std::string & key, float value) { if (key == "x") { T_camera_lidar_(0, 3) += value; @@ -235,7 +237,7 @@ void CalibrationNode::process_command(const ssct_interfaces::msg::CalibrationCom save_result(); } else if (msg.command == ssct_interfaces::msg::CalibrationCommand::CUSTOM_KEY_VAULE) { // update calibration data - if (update_calibration_params(msg.key, msg.value)) { + if (update_calib_param_manager(msg.key, msg.value)) { state_ = ssct_interfaces::msg::CalibrationStatus::CALIBRATING; } else { RCLCPP_FATAL(node_->get_logger(), "undefined custom key value command: %s", msg.key.c_str()); @@ -261,17 +263,17 @@ void CalibrationNode::save_result() return; } // - auto calibration_params = std::make_shared(); + auto calib_param_manager = std::make_shared(); if (std::filesystem::exists(output_calibration_file_)) { - if (!calibration_params->load(output_calibration_file_)) { + if (!calib_param_manager->load(output_calibration_file_)) { RCLCPP_FATAL( node_->get_logger(), "failed to load existed calibration data, %s", - calibration_params->error_message().c_str()); + calib_param_manager->error_message().c_str()); return; } } - calibration_params->add_extrinsic_param(camera_frame_id_, lidar_frame_id_, T_camera_lidar_); - if (calibration_params->save(output_calibration_file_)) { + calib_param_manager->add_extrinsic_param(camera_frame_id_, lidar_frame_id_, T_camera_lidar_); + if (calib_param_manager->save(output_calibration_file_)) { RCLCPP_INFO( node_->get_logger(), "successed to save result: %s", output_calibration_file_.c_str()); } else {