add calib param manager and imu intrinsic param (#10)

* add calib param

* add calib param manager

* add imu intrinsic param

* update doc

* code format

* update doc

Author: gezp

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`坐标系下的位姿变换。

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<SensorData> sensor_data_buffer_;
-  std::shared_ptr<ssct_common::CalibrationParams> calibration_params_;
+  std::shared_ptr<ssct_common::CalibParamManager> calib_param_manager_;
   uint8_t state_;
   bool success_{false};
   bool is_auto_mode_{true};

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<PinholeCalibrator>(config_node["pinhole_calibrator"]);
-  calibration_params_ = std::make_shared<ssct_common::CalibrationParams>();
+  calib_param_manager_ = std::make_shared<ssct_common::CalibParamManager>();
   // 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());

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
 )

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 <Eigen/Geometry>
+
+#include <string>
+#include <vector>
+
+namespace ssct_common
+{
+
+struct CameraIntrinsicParam
+{
+  std::string frame_id;
+  int height;
+  int width;
+  std::string type;
+  std::vector<double> intrinsics;
+  std::vector<double> distortion_coeffs;
+};
+
+}  // namespace ssct_common

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 <Eigen/Geometry>
+
+#include <string>
+
+namespace ssct_common
+{
+
+struct ExtrinsicParam
+{
+  std::string frame_id;
+  std::string child_frame_id;
+  Eigen::Matrix4d transform;
+};
+
+}  // namespace ssct_common

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 <Eigen/Geometry>
+
+#include <string>
+#include <vector>
+
+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

ssct_common/include/ssct_common/calibration_params.hpp renamed to ssct_common/include/ssct_common/calib_param_manager.hpp

@@ -21,41 +21,33 @@
 #include <map>
 #include <utility>
 
-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<double> intrinsics;
-  std::vector<double> 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<std::string, CameraIntrinsicParam> camera_intrinsic_params_;
+  std::map<std::string, ImuIntrinsicParam> imu_intrinsic_params_;
   std::map<std::string, ExtrinsicParam> extrinsic_params_;
   std::string error_message_;
 };