From 517b767a8eabd6240e902eafea477fc7256652c0 Mon Sep 17 00:00:00 2001 From: Nathan Shankar Date: Wed, 16 Apr 2025 17:06:58 +0100 Subject: [PATCH 1/4] modified: ov_init/src/ceres/State_JPLQuatLocal.h modified: ov_msckf/src/ros/ROS2Visualizer.h modified: ov_msckf/src/ros/ROSVisualizerHelper.h --- ov_init/src/ceres/State_JPLQuatLocal.h | 40 +++++++++++++++----------- ov_msckf/src/ros/ROS2Visualizer.h | 6 ++-- ov_msckf/src/ros/ROSVisualizerHelper.h | 2 +- 3 files changed, 28 insertions(+), 20 deletions(-) diff --git a/ov_init/src/ceres/State_JPLQuatLocal.h b/ov_init/src/ceres/State_JPLQuatLocal.h index e6b3d01e..2f83861a 100644 --- a/ov_init/src/ceres/State_JPLQuatLocal.h +++ b/ov_init/src/ceres/State_JPLQuatLocal.h @@ -29,7 +29,7 @@ namespace ov_init { /** * @brief JPL quaternion CERES state parameterization */ -class State_JPLQuatLocal : public ceres::LocalParameterization { +class State_JPLQuatLocal : public ceres::Manifold { public: /** * @brief State update function for a JPL quaternion representation. @@ -41,24 +41,32 @@ class State_JPLQuatLocal : public ceres::LocalParameterization { * \bar{q}=norm\Big(\begin{bmatrix} 0.5*\mathbf{\theta_{dx}} \\ 1 \end{bmatrix}\Big) \hat{\bar{q}} * @f] */ - bool Plus(const double *x, const double *delta, double *x_plus_delta) const override; + int AmbientSize() const override { + return 4; // For example, if this represents a 4D state (quaternion) +} - /** - * @brief Computes the jacobian in respect to the local parameterization - * - * This essentially "tricks" ceres. - * Instead of doing what ceres wants: - * dr/dlocal= dr/dglobal * dglobal/dlocal - * - * We instead directly do: - * dr/dlocal= [ dr/dlocal, 0] * [I; 0]= dr/dlocal. - * Therefore we here define dglobal/dlocal= [I; 0] - */ - bool ComputeJacobian(const double *x, double *jacobian) const override; +int TangentSize() const override { + return 3; // Local perturbation size, typically 3 for a rotation +} + +bool PlusJacobian(const double* x, double* jacobian) const override { + // Implement the jacobian computation for the Plus operation + return true; +} +bool Plus(const double* x, const double* delta, double* x_plus_delta) const override; +bool ComputeJacobian(const double* x, double* jacobian) const; + + +bool Minus(const double* y, const double* x, double* delta) const override { + // Implement the minus operation + return true; +} - int GlobalSize() const override { return 4; }; +bool MinusJacobian(const double* x, double* jacobian) const override { + // Implement the jacobian for the Minus operation + return true; +} - int LocalSize() const override { return 3; }; }; } // namespace ov_init diff --git a/ov_msckf/src/ros/ROS2Visualizer.h b/ov_msckf/src/ros/ROS2Visualizer.h index a737f80a..f04c614c 100644 --- a/ov_msckf/src/ros/ROS2Visualizer.h +++ b/ov_msckf/src/ros/ROS2Visualizer.h @@ -25,7 +25,7 @@ #include #include #include -#include +#include #include #include #include @@ -42,7 +42,7 @@ #include #include #include -#include +#include #include #include @@ -53,7 +53,7 @@ #include #include #include -#include +#include namespace ov_core { class YamlParser; diff --git a/ov_msckf/src/ros/ROSVisualizerHelper.h b/ov_msckf/src/ros/ROSVisualizerHelper.h index 14c1514c..4dff3043 100644 --- a/ov_msckf/src/ros/ROSVisualizerHelper.h +++ b/ov_msckf/src/ros/ROSVisualizerHelper.h @@ -34,7 +34,7 @@ #include #include #include -#include +#include #endif namespace ov_type { From acb12cc0d33de413b1157fc22c51ee7e032eee6e Mon Sep 17 00:00:00 2001 From: Nathan Shankar <66565433+nathanshankar@users.noreply.github.com> Date: Wed, 16 Apr 2025 17:09:39 +0100 Subject: [PATCH 2/4] Update ReadMe.md --- ReadMe.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ReadMe.md b/ReadMe.md index 1448a49c..a28fd04a 100644 --- a/ReadMe.md +++ b/ReadMe.md @@ -1,4 +1,4 @@ -# OpenVINS +# OpenVINS - Added support for ROS2 Jazzy + Ubuntu 24.04 [![ROS 1 Workflow](https://github.com/rpng/open_vins/actions/workflows/build_ros1.yml/badge.svg)](https://github.com/rpng/open_vins/actions/workflows/build_ros1.yml) [![ROS 2 Workflow](https://github.com/rpng/open_vins/actions/workflows/build_ros2.yml/badge.svg)](https://github.com/rpng/open_vins/actions/workflows/build_ros2.yml) From 7dfc1fce48caf728ac30d45f8c9fce1cbce1a498 Mon Sep 17 00:00:00 2001 From: Nathan Shankar Date: Thu, 17 Apr 2025 10:44:33 +0100 Subject: [PATCH 3/4] support for ros2 jazzy on ubuntu 24.04 --- config/rs_d455/estimator_config.yaml | 10 ++--- config/rs_d455/kalibr_imu_chain.yaml | 22 ++++++----- config/rs_d455/kalibr_imucam_chain.yaml | 52 ++++++++++++++++++++----- ov_msckf/src/update/UpdaterHelper.cpp | 13 ++++++- 4 files changed, 68 insertions(+), 29 deletions(-) diff --git a/config/rs_d455/estimator_config.yaml b/config/rs_d455/estimator_config.yaml index 22629534..31999766 100644 --- a/config/rs_d455/estimator_config.yaml +++ b/config/rs_d455/estimator_config.yaml @@ -27,7 +27,7 @@ feat_rep_aruco: "ANCHORED_MSCKF_INVERSE_DEPTH" # zero velocity update parameters we can use # we support either IMU-based or disparity detection. -try_zupt: false +try_zupt: true zupt_chi2_multipler: 0 # set to 0 for only disp-based zupt_max_velocity: 0.1 zupt_noise_multiplier: 10 @@ -38,7 +38,7 @@ zupt_only_at_beginning: false # ================================================================== init_window_time: 2.0 # how many seconds to collect initialization information -init_imu_thresh: 1.5 # threshold for variance of the accelerometer to detect a "jerk" in motion +init_imu_thresh: 2.5 # threshold for variance of the accelerometer to detect a "jerk" in motion init_max_disparity: 10.0 # max disparity to consider the platform stationary (dependent on resolution) init_max_features: 50 # how many features to track during initialization (saves on computation) @@ -112,8 +112,4 @@ use_mask: false # imu and camera spacial-temporal # imu config should also have the correct noise values relative_config_imu: "kalibr_imu_chain.yaml" -relative_config_imucam: "kalibr_imucam_chain.yaml" - - - - +relative_config_imucam: "kalibr_imucam_chain.yaml" \ No newline at end of file diff --git a/config/rs_d455/kalibr_imu_chain.yaml b/config/rs_d455/kalibr_imu_chain.yaml index 6dac5632..64f27c12 100644 --- a/config/rs_d455/kalibr_imu_chain.yaml +++ b/config/rs_d455/kalibr_imu_chain.yaml @@ -13,21 +13,23 @@ imu0: #gyroscope_noise_density: 0.00010272083263292572 #gyroscope_random_walk: 1.1106223553679963e-06 # Inflated values (to account for unmodelled effects) - # - white noise multiplied by 2 - # - bias random walk multiplied by 10 - accelerometer_noise_density: 0.00207649074 - accelerometer_random_walk: 0.00041327852 - gyroscope_noise_density: 0.00020544166 - gyroscope_random_walk: 0.00001110622 - rostopic: /d455/imu + # density x5 + # walk x10 + #Accelerometer + accelerometer_noise_density: 0.018325460766195713 + accelerometer_random_walk: 0.0031526548679969497 + #Gyroscope + gyroscope_noise_density: 0.005658445654838128 + gyroscope_random_walk: 4.9244827968800026e-04 + rostopic: /camera/imu time_offset: 0.0 - update_rate: 400 + update_rate: 200 # three different modes supported: # "calibrated" (same as "kalibr"), "kalibr", "rpng" model: "kalibr" # how to get from Kalibr imu.yaml result file: # - Tw is imu0:gyroscopes:M: - # - R_IMUtoGYRO: is imu0:gyroscopes:C_gyro_i: + # - R_IMUtoGYRO: is imu0:gyroscopes:M: # - Ta is imu0:accelerometers:M: # - R_IMUtoACC not used by Kalibr # - Tg is imu0:gyroscopes:A: @@ -50,4 +52,4 @@ imu0: Tg: - [ 0.0, 0.0, 0.0 ] - [ 0.0, 0.0, 0.0 ] - - [ 0.0, 0.0, 0.0 ] + - [ 0.0, 0.0, 0.0 ] \ No newline at end of file diff --git a/config/rs_d455/kalibr_imucam_chain.yaml b/config/rs_d455/kalibr_imucam_chain.yaml index 8dfff7fc..9ca4b912 100644 --- a/config/rs_d455/kalibr_imucam_chain.yaml +++ b/config/rs_d455/kalibr_imucam_chain.yaml @@ -1,19 +1,51 @@ %YAML:1.0 - cam0: T_cam_imu: - - [0.9999654398038452, 0.007342326779113337, -0.003899927610975742, -0.027534314618518095] - - [-0.0073452195116216765, 0.9999727585590525, -0.0007279355223411334, -0.0030587146933711722] - - [0.0038944766308488753, 0.0007565561891287445, 0.9999921303062861, -0.023605118842939803] + - [0.9999942992129773, -0.0006837030318090254, 0.003306673813320594, 0.01734357246126995] + - [0.0006819183997666188, 0.9999996212593953, 0.000540803438387852, -0.008519633421954392] + - [-0.003307042309899398, -0.0005385454736672996, 0.9999943867042107, -0.018723905687986195] - [0.0, 0.0, 0.0, 1.0] - cam_overlaps: [] + cam_overlaps: [1] camera_model: pinhole - distortion_coeffs: [-0.045761895748285604, 0.03423951132164367, -0.00040139057556727315, 0.000431371425853453] + distortion_coeffs: [0.0019712980218442736, -0.0011090942743057983, -0.0006211171700097682, -0.0034327653409294196] distortion_model: radtan - intrinsics: [416.85223429743274, 414.92069080087543, 421.02459311003213, 237.76180565241077] - resolution: [848, 480] - rostopic: /d455/color/image_raw - timeshift_cam_imu: 0.002524377913673846 + intrinsics: [320.17488748238287, 320.0283628174116, 314.0668795454674, 180.36893447271257] + resolution: [640, 360] + rostopic: /camera/infra1/image_rect_raw + timeshift_cam_imu: 0.004639958649183853 +cam1: + T_cam_imu: + - [0.9999855287596171, -0.0003605813977934117, 0.00536770457504073, -0.07667191595392714] + - [0.000365189906339375, 0.9999995655732808, -0.0008576068926449692, -0.008542662389807396] + - [-0.005367393006074334, 0.0008595547135404881, 0.9999852260198713, -0.017739927010989854] + - [0.0, 0.0, 0.0, 1.0] + T_cn_cnm1: + - [0.9999978238338454, 0.00032423014342537217, 0.0020608741811117422, -0.09397410073693341] + - [-0.00032135073657469737, 0.9999989720658036, -0.0013973550168239824, -4.36282992379309e-05] + - [-0.002061325127286215, 0.0013966897125111987, 0.99999690009348, 0.0010315706607051294] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [0] + camera_model: pinhole + distortion_coeffs: [0.00415520544025779, -0.003951065114859482, -0.00042589527047545586, -0.00442085250772721] + distortion_model: radtan + intrinsics: [319.69438067702725, 320.1948319604044, 313.46161305104766, 180.7178241492703] + resolution: [640, 360] + rostopic: /camera/infra2/image_rect_raw + timeshift_cam_imu: 0.005012709485041032 +# cam0: +# T_cam_imu: +# - [0.9999654398038452, 0.007342326779113337, -0.003899927610975742, -0.027534314618518095] +# - [-0.0073452195116216765, 0.9999727585590525, -0.0007279355223411334, -0.0030587146933711722] +# - [0.0038944766308488753, 0.0007565561891287445, 0.9999921303062861, -0.023605118842939803] +# - [0.0, 0.0, 0.0, 1.0] +# cam_overlaps: [] +# camera_model: pinhole +# distortion_coeffs: [-0.045761895748285604, 0.03423951132164367, -0.00040139057556727315, 0.000431371425853453] +# distortion_model: radtan +# intrinsics: [416.85223429743274, 414.92069080087543, 421.02459311003213, 237.76180565241077] +# resolution: [848, 480] +# rostopic: /d455/color/image_raw +# timeshift_cam_imu: 0.002524377913673846 diff --git a/ov_msckf/src/update/UpdaterHelper.cpp b/ov_msckf/src/update/UpdaterHelper.cpp index b36c004f..7cdbf20c 100644 --- a/ov_msckf/src/update/UpdaterHelper.cpp +++ b/ov_msckf/src/update/UpdaterHelper.cpp @@ -371,7 +371,16 @@ void UpdaterHelper::get_feature_jacobian_full(std::shared_ptr state, Upda dzn_dpfc << 1 / p_FinCi(2), 0, -p_FinCi(0) / (p_FinCi(2) * p_FinCi(2)), 0, 1 / p_FinCi(2), -p_FinCi(1) / (p_FinCi(2) * p_FinCi(2)); // Derivative of p_FinCi in respect to p_FinIi - Eigen::MatrixXd dpfc_dpfg = R_ItoC * R_GtoIi; + std::cout << "R_ItoC: " << R_ItoC.rows() << "x" << R_ItoC.cols() << std::endl; + std::cout << "R_GtoIi: " << R_GtoIi.rows() << "x" << R_GtoIi.cols() << std::endl; + + std::cout << "R_ItoC matrix: " << std::endl << R_ItoC << std::endl; + std::cout << "R_GtoIi matrix: " << std::endl << R_GtoIi << std::endl; + + Eigen::MatrixXd dpfc_dpfg(3, 3); + dpfc_dpfg.setZero(); + dpfc_dpfg = R_ItoC * R_GtoIi; + std::cout<<"fixed"< state, Upda // Calculate the Jacobian Eigen::MatrixXd dpfc_dcalib = Eigen::MatrixXd::Zero(3, 6); - dpfc_dcalib.block(0, 0, 3, 3) = skew_x(p_FinCi - p_IinC); + dpfc_dcalib.block(0, 0, 3, 3).noalias() = skew_x(p_FinCi - p_IinC); dpfc_dcalib.block(0, 3, 3, 3) = Eigen::Matrix::Identity(); // Chainrule it and add it to the big jacobian From 1698e2515868f1364c76e6944f72e143d300cc3f Mon Sep 17 00:00:00 2001 From: Nathan Shankar Date: Thu, 17 Apr 2025 15:50:40 +0100 Subject: [PATCH 4/4] new file: config/rs_d455_mono/estimator_config.yaml new file: config/rs_d455_mono/kalibr_imu_chain.yaml new file: config/rs_d455_mono/kalibr_imucam_chain.yaml renamed: config/rs_d455/launch_d455.example -> config/rs_d455_mono/launch_d455.example renamed: config/rs_d455/estimator_config.yaml -> config/rs_d455_stereo/estimator_config.yaml renamed: config/rs_d455/kalibr_imu_chain.yaml -> config/rs_d455_stereo/kalibr_imu_chain.yaml renamed: config/rs_d455/kalibr_imucam_chain.yaml -> config/rs_d455_stereo/kalibr_imucam_chain.yaml new file: config/rs_d455_stereo/launch_d455.example --- config/rs_d455_mono/estimator_config.yaml | 119 ++++++++++++++++++ config/rs_d455_mono/kalibr_imu_chain.yaml | 44 +++++++ config/rs_d455_mono/kalibr_imucam_chain.yaml | 19 +++ .../launch_d455.example | 0 .../estimator_config.yaml | 0 .../kalibr_imu_chain.yaml | 0 .../kalibr_imucam_chain.yaml | 0 config/rs_d455_stereo/launch_d455.example | 110 ++++++++++++++++ 8 files changed, 292 insertions(+) create mode 100644 config/rs_d455_mono/estimator_config.yaml create mode 100644 config/rs_d455_mono/kalibr_imu_chain.yaml create mode 100644 config/rs_d455_mono/kalibr_imucam_chain.yaml rename config/{rs_d455 => rs_d455_mono}/launch_d455.example (100%) rename config/{rs_d455 => rs_d455_stereo}/estimator_config.yaml (100%) rename config/{rs_d455 => rs_d455_stereo}/kalibr_imu_chain.yaml (100%) rename config/{rs_d455 => rs_d455_stereo}/kalibr_imucam_chain.yaml (100%) create mode 100644 config/rs_d455_stereo/launch_d455.example diff --git a/config/rs_d455_mono/estimator_config.yaml b/config/rs_d455_mono/estimator_config.yaml new file mode 100644 index 00000000..e2f779c1 --- /dev/null +++ b/config/rs_d455_mono/estimator_config.yaml @@ -0,0 +1,119 @@ +%YAML:1.0 # need to specify the file type at the top! + +verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT + +use_fej: true # if first-estimate Jacobians should be used (enable for good consistency) +integration: "rk4" # discrete, rk4, analytical (if rk4 or analytical used then analytical covariance propagation is used) +use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints between pairs +max_cameras: 1 # how many cameras we have 1 = mono, 2 = stereo, >2 = binocular (all mono tracking) + +calib_cam_extrinsics: true # if the transform between camera and IMU should be optimized R_ItoC, p_CinI +calib_cam_intrinsics: true # if camera intrinsics should be optimized (focal, center, distortion) +calib_cam_timeoffset: true # if timeoffset between camera and IMU should be optimized +calib_imu_intrinsics: false # if imu intrinsics should be calibrated (rotation and skew-scale matrix) +calib_imu_g_sensitivity: false # if gyroscope gravity sensitivity (Tg) should be calibrated + +max_clones: 11 # how many clones in the sliding window +max_slam: 25 # number of features in our state vector +max_slam_in_update: 25 # update can be split into sequential updates of batches, how many in a batch +max_msckf_in_update: 20 # how many MSCKF features to use in the update +dt_slam_delay: 2 # delay before initializing (helps with stability from bad initialization...) + +gravity_mag: 9.8065 # magnitude of gravity in this location + +feat_rep_msckf: "GLOBAL_3D" +feat_rep_slam: "GLOBAL_3D" +feat_rep_aruco: "GLOBAL_3D" + +# zero velocity update parameters we can use +# we support either IMU-based or disparity detection. +try_zupt: false +zupt_chi2_multipler: 0 # set to 0 for only disp-based +zupt_max_velocity: 0.1 +zupt_noise_multiplier: 50 +zupt_max_disparity: 1.5 # set to 0 for only imu-based +zupt_only_at_beginning: true + +# ================================================================== +# ================================================================== + +init_window_time: 2.0 # how many seconds to collect initialization information +init_imu_thresh: 0.5 # threshold for variance of the accelerometer to detect a "jerk" in motion +init_max_disparity: 4.0 # max disparity to consider the platform stationary (dependent on resolution) +init_max_features: 50 # how many features to track during initialization (saves on computation) + +init_dyn_use: false # if dynamic initialization should be used +init_dyn_mle_opt_calib: false # if we should optimize calibration during intialization (not recommended) +init_dyn_mle_max_iter: 50 # how many iterations the MLE refinement should use (zero to skip the MLE) +init_dyn_mle_max_time: 0.05 # how many seconds the MLE should be completed in +init_dyn_mle_max_threads: 6 # how many threads the MLE should use +init_dyn_num_pose: 6 # number of poses to use within our window time (evenly spaced) +init_dyn_min_deg: 10.0 # orientation change needed to try to init + +init_dyn_inflation_ori: 10 # what to inflate the recovered q_GtoI covariance by +init_dyn_inflation_vel: 100 # what to inflate the recovered v_IinG covariance by +init_dyn_inflation_bg: 10 # what to inflate the recovered bias_g covariance by +init_dyn_inflation_ba: 100 # what to inflate the recovered bias_a covariance by +init_dyn_min_rec_cond: 1e-12 # reciprocal condition number thresh for info inversion + +init_dyn_bias_g: [ 0.0, 0.0, 0.0 ] # initial gyroscope bias guess +init_dyn_bias_a: [ 0.0, 0.0, 0.0 ] # initial accelerometer bias guess + +# ================================================================== +# ================================================================== + +record_timing_information: false # if we want to record timing information of the method +record_timing_filepath: "/tmp/traj_timing.txt" # https://docs.openvins.com/eval-timing.html#eval-ov-timing-flame + +# if we want to save the simulation state and its diagional covariance +# use this with rosrun ov_eval error_simulation +save_total_state: false +filepath_est: "/tmp/ov_estimate.txt" +filepath_std: "/tmp/ov_estimate_std.txt" +filepath_gt: "/tmp/ov_groundtruth.txt" + +# ================================================================== +# ================================================================== + +# our front-end feature tracking parameters +# we have a KLT and descriptor based (KLT is better implemented...) +use_klt: true # if true we will use KLT, otherwise use a ORB descriptor + robust matching +num_pts: 200 # number of points (per camera) we will extract and try to track +fast_threshold: 30 # threshold for fast extraction (warning: lower threshs can be expensive) +grid_x: 8 # extraction sub-grid count for horizontal direction (uniform tracking) +grid_y: 8 # extraction sub-grid count for vertical direction (uniform tracking) +min_px_dist: 20 # distance between features (features near each other provide less information) +knn_ratio: 0.70 # descriptor knn threshold for the top two descriptor matches +track_frequency: 31.0 # frequency we will perform feature tracking at (in frames per second / hertz) +downsample_cameras: false # will downsample image in half if true +num_opencv_threads: 4 # -1: auto, 0-1: serial, >1: number of threads +histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE + +# aruco tag tracker for the system +# DICT_6X6_1000 from https://chev.me/arucogen/ +use_aruco: false +num_aruco: 1024 +downsize_aruco: true + +# ================================================================== +# ================================================================== + +# camera noises and chi-squared threshold multipliers +up_msckf_sigma_px: 1 +up_msckf_chi2_multipler: 1 +up_slam_sigma_px: 1 +up_slam_chi2_multipler: 1 +up_aruco_sigma_px: 1 +up_aruco_chi2_multipler: 1 + +# masks for our images +use_mask: false + +# imu and camera spacial-temporal +# imu config should also have the correct noise values +relative_config_imu: "kalibr_imu_chain.yaml" +relative_config_imucam: "kalibr_imucam_chain.yaml" + + + + diff --git a/config/rs_d455_mono/kalibr_imu_chain.yaml b/config/rs_d455_mono/kalibr_imu_chain.yaml new file mode 100644 index 00000000..7b184cf0 --- /dev/null +++ b/config/rs_d455_mono/kalibr_imu_chain.yaml @@ -0,0 +1,44 @@ +%YAML:1.0 + +imu0: + T_i_b: + - [1.0, 0.0, 0.0, 0.0] + - [0.0, 1.0, 0.0, 0.0] + - [0.0, 0.0, 1.0, 0.0] + - [0.0, 0.0, 0.0, 1.0] + accelerometer_noise_density: 0.00207649074 + accelerometer_random_walk: 0.00041327852 + gyroscope_noise_density: 0.00020544166 + gyroscope_random_walk: 1.110622e-05 + rostopic: /d455/imu + time_offset: 0.0 + update_rate: 400 + # three different modes supported: + # "calibrated" (same as "kalibr"), "kalibr", "rpng" + model: "kalibr" + # how to get from Kalibr imu.yaml result file: + # - Tw is imu0:gyroscopes:M: + # - R_IMUtoGYRO: is imu0:gyroscopes:C_gyro_i: + # - Ta is imu0:accelerometers:M: + # - R_IMUtoACC not used by Kalibr + # - Tg is imu0:gyroscopes:A: + Tw: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + R_IMUtoGYRO: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + Ta: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + R_IMUtoACC: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + Tg: + - [ 0.0, 0.0, 0.0 ] + - [ 0.0, 0.0, 0.0 ] + - [ 0.0, 0.0, 0.0 ] diff --git a/config/rs_d455_mono/kalibr_imucam_chain.yaml b/config/rs_d455_mono/kalibr_imucam_chain.yaml new file mode 100644 index 00000000..8dfff7fc --- /dev/null +++ b/config/rs_d455_mono/kalibr_imucam_chain.yaml @@ -0,0 +1,19 @@ +%YAML:1.0 + + +cam0: + T_cam_imu: + - [0.9999654398038452, 0.007342326779113337, -0.003899927610975742, -0.027534314618518095] + - [-0.0073452195116216765, 0.9999727585590525, -0.0007279355223411334, -0.0030587146933711722] + - [0.0038944766308488753, 0.0007565561891287445, 0.9999921303062861, -0.023605118842939803] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [] + camera_model: pinhole + distortion_coeffs: [-0.045761895748285604, 0.03423951132164367, -0.00040139057556727315, 0.000431371425853453] + distortion_model: radtan + intrinsics: [416.85223429743274, 414.92069080087543, 421.02459311003213, 237.76180565241077] + resolution: [848, 480] + rostopic: /d455/color/image_raw + timeshift_cam_imu: 0.002524377913673846 + + diff --git a/config/rs_d455/launch_d455.example b/config/rs_d455_mono/launch_d455.example similarity index 100% rename from config/rs_d455/launch_d455.example rename to config/rs_d455_mono/launch_d455.example diff --git a/config/rs_d455/estimator_config.yaml b/config/rs_d455_stereo/estimator_config.yaml similarity index 100% rename from config/rs_d455/estimator_config.yaml rename to config/rs_d455_stereo/estimator_config.yaml diff --git a/config/rs_d455/kalibr_imu_chain.yaml b/config/rs_d455_stereo/kalibr_imu_chain.yaml similarity index 100% rename from config/rs_d455/kalibr_imu_chain.yaml rename to config/rs_d455_stereo/kalibr_imu_chain.yaml diff --git a/config/rs_d455/kalibr_imucam_chain.yaml b/config/rs_d455_stereo/kalibr_imucam_chain.yaml similarity index 100% rename from config/rs_d455/kalibr_imucam_chain.yaml rename to config/rs_d455_stereo/kalibr_imucam_chain.yaml diff --git a/config/rs_d455_stereo/launch_d455.example b/config/rs_d455_stereo/launch_d455.example new file mode 100644 index 00000000..51322571 --- /dev/null +++ b/config/rs_d455_stereo/launch_d455.example @@ -0,0 +1,110 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file