From 91f072afb46c572d65b2f4de27d0e8cccbf11637 Mon Sep 17 00:00:00 2001 From: ori Date: Tue, 29 Jul 2025 13:51:22 +0100 Subject: [PATCH 01/11] add calibrated aria intrinsics to config path --- config/aria/estimator_config.yaml | 123 +++++++++++++++++++++++++++ config/aria/kalibr_imu_chain.yaml | 37 ++++++++ config/aria/kalibr_imucam_chain.yaml | 30 +++++++ 3 files changed, 190 insertions(+) create mode 100644 config/aria/estimator_config.yaml create mode 100644 config/aria/kalibr_imu_chain.yaml create mode 100644 config/aria/kalibr_imucam_chain.yaml diff --git a/config/aria/estimator_config.yaml b/config/aria/estimator_config.yaml new file mode 100644 index 000000000..4cb5a9ab3 --- /dev/null +++ b/config/aria/estimator_config.yaml @@ -0,0 +1,123 @@ +%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: false # if we have more than 1 camera, if we should try to track stereo constraints +max_cameras: 2 # how many cameras we have 1 = mono, 2 = stereo, >2 = binocular (all mono tracking) + +calib_cam_extrinsics: false # degenerate motion +calib_cam_intrinsics: true +calib_cam_timeoffset: true # degenerate motion +calib_imu_intrinsics: false +calib_imu_g_sensitivity: false + +max_clones: 11 +max_slam: 50 +max_slam_in_update: 25 +max_msckf_in_update: 50 +dt_slam_delay: 1 + +gravity_mag: 9.81 + +feat_rep_msckf: "ANCHORED_MSCKF_INVERSE_DEPTH" +feat_rep_slam: "ANCHORED_MSCKF_INVERSE_DEPTH" +feat_rep_aruco: "ANCHORED_MSCKF_INVERSE_DEPTH" + +# zero velocity update parameters we can use +# we support either IMU-based or disparity detection. +try_zupt: true +zupt_chi2_multipler: 0.5 # set to 0 for only disp-based +zupt_max_velocity: 0.1 +zupt_noise_multiplier: 1 +zupt_max_disparity: 0.4 # set to 0 for only imu-based +zupt_only_at_beginning: false + +# ================================================================== +# ================================================================== + +init_window_time: 2.0 +init_imu_thresh: 0.5 +init_max_disparity: 1.5 +init_max_features: 50 + +init_dyn_use: true +init_dyn_mle_opt_calib: false +init_dyn_mle_max_iter: 50 +init_dyn_mle_max_time: 0.05 +init_dyn_mle_max_threads: 6 +init_dyn_num_pose: 6 +init_dyn_min_deg: 5.0 # traj is mostly straight line motion + +init_dyn_inflation_ori: 10 +init_dyn_inflation_vel: 100 +init_dyn_inflation_bg: 10 +init_dyn_inflation_ba: 100 +init_dyn_min_rec_cond: 1e-20 # traj is mostly straight line motion + +init_dyn_bias_g: [0.0, 0.0, 0.0] +init_dyn_bias_a: [0.0, 0.0, 0.0] + + +# ================================================================== +# ================================================================== + +record_timing_information: false +record_timing_filepath: "/tmp/traj_timing.txt" + +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 +num_pts: 200 +fast_threshold: 30 +grid_x: 5 +grid_y: 5 +min_px_dist: 20 +knn_ratio: 0.65 +track_frequency: 31.0 +downsample_cameras: false +num_opencv_threads: 4 # -1: auto, 0-1: serial, >1: number of threads +histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE + +fi_min_dist: 0.25 +fi_max_dist: 150.0 +fi_max_baseline: 200 +fi_max_cond_number: 20000 +fi_triangulate_1d: false + +# 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.5 +up_msckf_chi2_multipler: 1 +up_slam_sigma_px: 1.5 +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/aria/kalibr_imu_chain.yaml b/config/aria/kalibr_imu_chain.yaml new file mode 100644 index 000000000..aa77bdab3 --- /dev/null +++ b/config/aria/kalibr_imu_chain.yaml @@ -0,0 +1,37 @@ +%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.010843559005996556 + accelerometer_random_walk: 48.7810758784797e-05 + gyroscope_noise_density: 0.002509873909262778 + gyroscope_random_walk: 22.7931834950152e-06 + model: calibrated + rostopic: /imu_right + time_offset: 0.0 + update_rate: 1000 + + 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 ] \ No newline at end of file diff --git a/config/aria/kalibr_imucam_chain.yaml b/config/aria/kalibr_imucam_chain.yaml new file mode 100644 index 000000000..d6443cc50 --- /dev/null +++ b/config/aria/kalibr_imucam_chain.yaml @@ -0,0 +1,30 @@ +%YAML:1.0 + +cam0: + T_cam_imu: + - [0.9782501693075571, 0.05018607300803547, -0.2012659045287295, 0.006435676135584448] + - [0.20742334480631525, -0.22979728423453438, 0.950877891312966, -0.10326121731212001] + - [0.0014704690054501979, -0.9719437052805396, -0.23520899533487083, -0.08721481970331113] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [1] + camera_model: pinhole + distortion_coeffs: [-0.03521708561298138, 0.13993068272515677, -0.1382658712726599, 0.054157317943265844] + distortion_model: equidistant + intrinsics: [241.78621144978587, 242.39804316458955, 317.3114290668952, 242.6087760646631] + resolution: [640, 480] + rostopic: /camera_slam_left/image + timeshift_cam_imu: 0.004270217900919078 +cam1: + T_cam_imu: + - [0.9874777193231493, 0.011345848775221044, -0.15735000971058993, 0.0015316425604920388] + - [0.010616662632099013, -0.9999286587861259, -0.005473921148227179, -0.010071955292996279] + - [-0.15740089045144923, 0.00373484320295129, -0.9875277265126997, -0.004942054217553331] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [0] + camera_model: pinhole + distortion_coeffs: [-0.026866455232950476, 0.10790535425845371, -0.08882084656895586, 0.02627360209817728] + distortion_model: equidistant + intrinsics: [242.1538223221408, 242.14114214084816, 317.8751234840723, 244.03555111568332] + resolution: [640, 480] + rostopic: /camera_slam_right/image + timeshift_cam_imu: 0.004075260451014888 \ No newline at end of file From 22e494d224237c9122c328c29920d8e8c5e2fb6d Mon Sep 17 00:00:00 2001 From: ori Date: Tue, 29 Jul 2025 13:52:32 +0100 Subject: [PATCH 02/11] add listen_runtime executable, launch file, and csv file utils --- ov_msckf/cmake/ROS2.cmake | 9 +- ov_msckf/launch/aria.launch.py | 127 +++++++++++++++ ov_msckf/msg/ros2/OVRuntimeStatus.msg | 6 +- ov_msckf/src/listen_runtime.cpp | 221 ++++++++++++++++++++++++++ ov_msckf/src/ros/ROS2Visualizer.cpp | 7 +- ov_msckf/src/ros/ROS2Visualizer.h | 3 +- ov_msckf/src/utils/csvfile.h | 114 +++++++++++++ 7 files changed, 482 insertions(+), 5 deletions(-) create mode 100644 ov_msckf/launch/aria.launch.py create mode 100644 ov_msckf/src/listen_runtime.cpp create mode 100644 ov_msckf/src/utils/csvfile.h diff --git a/ov_msckf/cmake/ROS2.cmake b/ov_msckf/cmake/ROS2.cmake index c0a5472d8..104ce8e5e 100644 --- a/ov_msckf/cmake/ROS2.cmake +++ b/ov_msckf/cmake/ROS2.cmake @@ -29,7 +29,7 @@ set(msg_files rosidl_generate_interfaces(${PROJECT_NAME} ${msg_files} - DEPENDENCIES std_msgs geometry_msgs + DEPENDENCIES std_msgs geometry_msgs sensor_msgs ) ament_export_dependencies(rosidl_default_runtime) @@ -131,6 +131,13 @@ target_link_libraries(test_sim_repeat ov_msckf_lib ${thirdparty_libraries}) target_link_libraries(test_sim_repeat ${cpp_typesupport_target}) install(TARGETS test_sim_repeat DESTINATION lib/${PROJECT_NAME}) +add_executable(listen_runtime src/listen_runtime.cpp) +ament_target_dependencies(listen_runtime ${ament_libraries}) +target_link_libraries(listen_runtime ov_msckf_lib ${thirdparty_libraries}) +target_link_libraries(listen_runtime ${cpp_typesupport_target}) +install(TARGETS listen_runtime DESTINATION lib/${PROJECT_NAME}) + + # Install launch and config directories install(DIRECTORY launch/ DESTINATION share/${PROJECT_NAME}/launch/) install(DIRECTORY ../config/ DESTINATION share/${PROJECT_NAME}/config/) diff --git a/ov_msckf/launch/aria.launch.py b/ov_msckf/launch/aria.launch.py new file mode 100644 index 000000000..fac0346d7 --- /dev/null +++ b/ov_msckf/launch/aria.launch.py @@ -0,0 +1,127 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, TextSubstitution +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory, get_package_prefix +import os +import sys + +launch_args = [ + DeclareLaunchArgument(name="namespace", default_value="ov_msckf", description="namespace"), + DeclareLaunchArgument( + name="ov_enable", default_value="true", description="enable OpenVINS node" + ), + DeclareLaunchArgument( + name="rviz_enable", default_value="false", description="enable rviz node" + ), + DeclareLaunchArgument( + name="config", + default_value="aria", + description="euroc_mav, tum_vi, rpng_aruco...", + ), + DeclareLaunchArgument( + name="config_path", + default_value="", + description="path to estimator_config.yaml. If not given, determined based on provided 'config' above", + ), + DeclareLaunchArgument( + name="verbosity", + default_value="INFO", + description="ALL, DEBUG, INFO, WARNING, ERROR, SILENT", + ), + DeclareLaunchArgument( + name="use_stereo", + default_value="false", + description="if we have more than 1 camera, if we should try to track stereo constraints between pairs", + ), + DeclareLaunchArgument( + name="max_cameras", + default_value="2", + description="how many cameras we have 1 = mono, 2 = stereo, >2 = binocular (all mono tracking)", + ), + DeclareLaunchArgument( + name="save_total_state", + default_value="false", + description="record the total state with calibration and features to a txt file", + ), + DeclareLaunchArgument( + name="save_path", + default_value="aria", + description="save path for csv files" + ) +] + +def launch_setup(context): + config_path = LaunchConfiguration("config_path").perform(context) + if not config_path: + configs_dir = os.path.join(get_package_share_directory("ov_msckf"), "config/") + available_configs = os.listdir(configs_dir) + config = LaunchConfiguration("config").perform(context) + if config in available_configs: + config_path = os.path.join( + get_package_share_directory("ov_msckf"), + "config",config,"estimator_config.yaml" + ) + else: + return [ + LogInfo( + msg="ERROR: unknown config: '{}' - Available configs are: {} - not starting OpenVINS".format( + config, ", ".join(available_configs) + ) + ) + ] + else: + if not os.path.isfile(config_path): + return [ + LogInfo( + msg="ERROR: config_path file: '{}' - does not exist. - not starting OpenVINS".format( + config_path) + ) + ] + node1 = Node( + package="ov_msckf", + executable="run_subscribe_msckf", + condition=IfCondition(LaunchConfiguration("ov_enable")), + namespace=LaunchConfiguration("namespace"), + output='screen', + parameters=[ + {"verbosity": LaunchConfiguration("verbosity")}, + {"use_stereo": LaunchConfiguration("use_stereo")}, + {"max_cameras": LaunchConfiguration("max_cameras")}, + {"save_total_state": LaunchConfiguration("save_total_state")}, + {"config_path": config_path}, + ], + ) + + node2 = Node( + package="rviz2", + executable="rviz2", + condition=IfCondition(LaunchConfiguration("rviz_enable")), + arguments=[ + "-d" + + os.path.join( + get_package_share_directory("ov_msckf"), "launch", "display_ros2.rviz" + ), + "--ros-args", + "--log-level", + "warn", + ], + ) + + node3 = Node( + package="ov_msckf", + executable="listen_runtime", + parameters=[ + {"save_path": LaunchConfiguration("save_path")} + ] + ) + + return [node1, node2, node3] + + +def generate_launch_description(): + opfunc = OpaqueFunction(function=launch_setup) + ld = LaunchDescription(launch_args) + ld.add_action(opfunc) + return ld diff --git a/ov_msckf/msg/ros2/OVRuntimeStatus.msg b/ov_msckf/msg/ros2/OVRuntimeStatus.msg index 4f44d7d46..8dbb0b40f 100644 --- a/ov_msckf/msg/ros2/OVRuntimeStatus.msg +++ b/ov_msckf/msg/ros2/OVRuntimeStatus.msg @@ -13,4 +13,8 @@ geometry_msgs/Vector3 bias_a geometry_msgs/Vector3 bias_g # Time offset base IMU to camera (t_imu = t_cam + t_off) -float64 t_offset_imu_cam \ No newline at end of file +float64 t_offset_imu_cam + +# Pointcloud for features (Eigen::Vector3d p_FinG; //=> position of feature F in the global frame G) +sensor_msgs/PointCloud2 cloud_msckf_features +sensor_msgs/PointCloud2 cloud_slam_features \ No newline at end of file diff --git a/ov_msckf/src/listen_runtime.cpp b/ov_msckf/src/listen_runtime.cpp new file mode 100644 index 000000000..a927104c4 --- /dev/null +++ b/ov_msckf/src/listen_runtime.cpp @@ -0,0 +1,221 @@ +/* + * Copyright (C) 2025 Jin Rhee + * Script for listening and saving OpenVINS' tracked features, IMU biases, + * and camera calibration info + * + * OpenVINS: An Open Platform for Visual-Inertial Research + * Copyright (C) 2018-2023 Patrick Geneva + * Copyright (C) 2018-2023 Guoquan Huang + * Copyright (C) 2018-2023 OpenVINS Contributors + * Copyright (C) 2018-2019 Kevin Eckenhoff + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "ov_msckf/msg/ov_runtime_status.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" + +#include "utils/csvfile.h" + +using namespace ov_msckf; + +class RuntimeSubscriber : public rclcpp::Node +{ + public: + RuntimeSubscriber() : Node("listen_runtime") + { + // Subscribe to OVRuntimeStatus + this->subOVRuntimeStatus_ = this->create_subscription( + "/ov_msckf/runtime_status", 10, std::bind(&RuntimeSubscriber::topic1_callback, this, std::placeholders::_1) + ); + + // Get save file path, create if it doesn't exist + this->declare_parameter("save_path", "unset_save_path"); + this->get_parameter("save_path", this->save_path); + + // Name save paths + std::string bias_path = this->save_path + "_bias.csv"; + std::string msckf_path = this->save_path + "_msckf.csv"; + std::string slam_path = this->save_path + "_slam.csv"; + + // Open csv files at save_path + this->bias_csv = std::make_unique(bias_path); + this->msckf_csv = std::make_unique(msckf_path); + this->slam_csv = std::make_unique(slam_path); + RCLCPP_INFO(this->get_logger(), "Created .csv files"); + + // Populate first lines + *(this->bias_csv) << "sec" << "nanosec" << "frame_id" + << "bias_a_x" << "bias_a_y" << "bias_a_z" + << "bias_g_x" << "bias_g_y" << "bias_g_z" + << "t_offset_imu_cam" << endrow; + + *(this->msckf_csv) << "sec" << "nanosec" << "frame_id" + << "msckf_num_points" << "x" << "y" << "z" << endrow; + + *(this->slam_csv) << "sec" << "nanosec" << "frame_id" + << "slam_num_points" << "x" << "y" << "z" << endrow; + } + + private: + void topic1_callback(const ov_msckf::msg::OVRuntimeStatus::SharedPtr status_msg) + { + + RCLCPP_INFO(this->get_logger(), "Received message on topic1"); + std::string sec = std::to_string(status_msg->header.stamp.sec); + std::string nanosec = std::to_string(status_msg->header.stamp.nanosec); + std::string frame_id = status_msg->header.frame_id; + + *(this->bias_csv) << sec << nanosec << frame_id; + *(this->msckf_csv) << sec << nanosec << frame_id; + *(this->slam_csv) << sec << nanosec << frame_id; + + // Bias CSV + std::string bias_a_x = std::to_string(status_msg->bias_a.x); + std::string bias_a_y = std::to_string(status_msg->bias_a.y); + std::string bias_a_z = std::to_string(status_msg->bias_a.z); + std::string bias_g_x = std::to_string(status_msg->bias_g.x); + std::string bias_g_y = std::to_string(status_msg->bias_g.y); + std::string bias_g_z = std::to_string(status_msg->bias_g.z); + std::string t_offset_imu_cam = std::to_string(status_msg->t_offset_imu_cam); + + *(this->bias_csv) << bias_a_x << bias_a_y << bias_a_z << bias_g_x << bias_g_y << bias_g_z << t_offset_imu_cam << endrow; + + // MSCKF CSV + sensor_msgs::msg::PointCloud2 cloud_msckf = status_msg->cloud_msckf_features; + + // Extract XYZ coordinates from cloud_msckf + if (cloud_msckf.width * cloud_msckf.height == 0) { + RCLCPP_WARN(this->get_logger(), "cloud_msckf_features is empty"); + *(this->msckf_csv) << "0" << endrow; + } + else { + // Find offsets for x, y, z fields + int offset_x = -1, offset_y = -1, offset_z = -1; + for (const auto &field : cloud_msckf.fields) { + if (field.name == "x") offset_x = field.offset; + if (field.name == "y") offset_y = field.offset; + if (field.name == "z") offset_z = field.offset; + } + if (offset_x == -1 || offset_y == -1 || offset_z == -1) { + RCLCPP_ERROR(this->get_logger(), "Could not find x/y/z fields in cloud_msckf_features"); + *(this->msckf_csv) << "0" << endrow; + } + else { + size_t point_step = cloud_msckf.point_step; + size_t num_points = cloud_msckf.width * cloud_msckf.height; + + *(this->msckf_csv) << num_points; + std::string x_str; + std::string y_str; + std::string z_str; + + for (size_t i = 0; i < num_points; ++i) { + const uint8_t* ptr = &cloud_msckf.data[i * point_step]; + float x = *reinterpret_cast(ptr + offset_x); + float y = *reinterpret_cast(ptr + offset_y); + float z = *reinterpret_cast(ptr + offset_z); + + x_str = std::to_string(x); + y_str = std::to_string(y); + z_str = std::to_string(z); + + // Save xyz to csv + *(this->msckf_csv) << x_str << y_str << z_str; + RCLCPP_INFO(this->get_logger(), "Point %zu: x=%f, y=%f, z=%f", i, x, y, z); + } + *(this->msckf_csv) << endrow; + } + } + + // SLAM CSV + sensor_msgs::msg::PointCloud2 cloud_slam = status_msg->cloud_slam_features; + + // Extract XYZ coordinates from cloud_slam + if (cloud_slam.width * cloud_slam.height == 0) { + RCLCPP_WARN(this->get_logger(), "cloud_slam_features is empty"); + *(this->slam_csv) << "0" << endrow; + } + else { + // Find offsets for x, y, z fields + int offset_x = -1, offset_y = -1, offset_z = -1; + for (const auto &field : cloud_slam.fields) { + if (field.name == "x") offset_x = field.offset; + if (field.name == "y") offset_y = field.offset; + if (field.name == "z") offset_z = field.offset; + } + if (offset_x == -1 || offset_y == -1 || offset_z == -1) { + RCLCPP_ERROR(this->get_logger(), "Could not find x/y/z fields in cloud_slam_features"); + *(this->slam_csv) << "0" << endrow; + } + else { + size_t point_step = cloud_slam.point_step; + size_t num_points = cloud_slam.width * cloud_slam.height; + + *(this->slam_csv) << num_points; + std::string x_str; + std::string y_str; + std::string z_str; + + for (size_t i = 0; i < num_points; ++i) { + const uint8_t* ptr = &cloud_slam.data[i * point_step]; + float x = *reinterpret_cast(ptr + offset_x); + float y = *reinterpret_cast(ptr + offset_y); + float z = *reinterpret_cast(ptr + offset_z); + + x_str = std::to_string(x); + y_str = std::to_string(y); + z_str = std::to_string(z); + + // Save xyz to csv + *(this->slam_csv) << x_str << y_str << z_str; + RCLCPP_INFO(this->get_logger(), "Point %zu: x=%f, y=%f, z=%f", i, x, y, z); + } + *(this->slam_csv) << endrow; + } + } + } + + rclcpp::Subscription::SharedPtr subOVRuntimeStatus_; + std::string save_path; + std::unique_ptr bias_csv; + std::unique_ptr msckf_csv; + std::unique_ptr slam_csv; +}; + +int main(int argc, char **argv) { + // Ensure we have a path, if the user passes it then we should use it + std::string save_path = "unset_save_path"; + if (argc > 1) { + save_path = argv[1]; + } + + // Launch our ros node + rclcpp::init(argc, argv); + + // Create our subscriber node + auto node = std::make_shared(); + + // Spin to keep the node alive and process callbacks + rclcpp::spin(node); + + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/ov_msckf/src/ros/ROS2Visualizer.cpp b/ov_msckf/src/ros/ROS2Visualizer.cpp index 80eb54255..e6bea5087 100644 --- a/ov_msckf/src/ros/ROS2Visualizer.cpp +++ b/ov_msckf/src/ros/ROS2Visualizer.cpp @@ -30,7 +30,6 @@ #include "utils/dataset_reader.h" #include "utils/print.h" #include "utils/sensor_data.h" -// #include "ov_msckf/msg/o_vruntime_status.hpp" using namespace ov_core; using namespace ov_type; @@ -705,6 +704,12 @@ void ROS2Visualizer::publish_features() { status_msg.num_msckf_features = feats_msckf.size(); status_msg.num_slam_features = feats_slam.size(); + // Get feature coordinates (global) + sensor_msgs::msg::PointCloud2 cloud_msckf = ROSVisualizerHelper::get_ros_pointcloud(_node, feats_msckf); + status_msg.cloud_msckf_features = cloud_msckf; + sensor_msgs::msg::PointCloud2 cloud_slam = ROSVisualizerHelper::get_ros_pointcloud(_node, feats_slam); + status_msg.cloud_slam_features = cloud_slam; + // Publish runtime status pub_status->publish(status_msg); // PRINT_DEBUG("Published runtime status with %ld MSCKF features and %ld SLAM features\n", diff --git a/ov_msckf/src/ros/ROS2Visualizer.h b/ov_msckf/src/ros/ROS2Visualizer.h index 96d8ab43f..dbc90b41e 100644 --- a/ov_msckf/src/ros/ROS2Visualizer.h +++ b/ov_msckf/src/ros/ROS2Visualizer.h @@ -54,9 +54,8 @@ #include #include #include -// #include #include -// #include + namespace ov_core { class YamlParser; struct CameraData; diff --git a/ov_msckf/src/utils/csvfile.h b/ov_msckf/src/utils/csvfile.h new file mode 100644 index 000000000..63bceb113 --- /dev/null +++ b/ov_msckf/src/utils/csvfile.h @@ -0,0 +1,114 @@ +// csvfile.h writer by rudolfovich https://gist.github.com/rudolfovich/f250900f1a833e715260a66c87369d15 +// Used with permission under BSD-3 clause + +#include +#include +#include +#include + +class csvfile; + +inline static csvfile& endrow(csvfile& file); +inline static csvfile& flush(csvfile& file); + +class csvfile +{ + std::ofstream fs_; + bool is_first_; + const std::string separator_; + const std::string escape_seq_; + const std::string special_chars_; +public: + csvfile(const std::string filename, const std::string separator = ";") + : fs_() + , is_first_(true) + , separator_(separator) + , escape_seq_("\"") + , special_chars_("\"") + { + fs_.exceptions(std::ios::failbit | std::ios::badbit); + fs_.open(filename); + } + + ~csvfile() + { + flush(); + fs_.close(); + } + + void flush() + { + fs_.flush(); + } + + void endrow() + { + fs_ << std::endl; + is_first_ = true; + } + + csvfile& operator << ( csvfile& (* val)(csvfile&)) + { + return val(*this); + } + + csvfile& operator << (const char * val) + { + return write(escape(val)); + } + + csvfile& operator << (const std::string & val) + { + return write(escape(val)); + } + + template + csvfile& operator << (const T& val) + { + return write(val); + } + +private: + template + csvfile& write (const T& val) + { + if (!is_first_) + { + fs_ << separator_; + } + else + { + is_first_ = false; + } + fs_ << val; + return *this; + } + + std::string escape(const std::string & val) + { + std::ostringstream result; + //result << '"'; + std::string::size_type to, from = 0u, len = val.length(); + while (from < len && + std::string::npos != (to = val.find_first_of(special_chars_, from))) + { + result << val.substr(from, to - from) << escape_seq_ << val[to]; + from = to + 1; + } + //result << val.substr(from) << '"'; + return result.str(); + } +}; + + +inline static csvfile& endrow(csvfile& file) +{ + file.endrow(); + return file; +} + +inline static csvfile& flush(csvfile& file) +{ + file.flush(); + return file; +} \ No newline at end of file From c93a3fdf0050e4839c6b1ae19440e1f913440556 Mon Sep 17 00:00:00 2001 From: ori Date: Tue, 29 Jul 2025 14:13:15 +0100 Subject: [PATCH 03/11] fix in csv file util --- ov_msckf/src/utils/csvfile.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ov_msckf/src/utils/csvfile.h b/ov_msckf/src/utils/csvfile.h index 63bceb113..3c0d4109b 100644 --- a/ov_msckf/src/utils/csvfile.h +++ b/ov_msckf/src/utils/csvfile.h @@ -95,7 +95,7 @@ class csvfile result << val.substr(from, to - from) << escape_seq_ << val[to]; from = to + 1; } - //result << val.substr(from) << '"'; + result << val.substr(from); // << '"'; return result.str(); } }; From 7c317db3244093d55aaab8368b4df0958e049404 Mon Sep 17 00:00:00 2001 From: ori Date: Wed, 30 Jul 2025 14:07:21 +0100 Subject: [PATCH 04/11] add new message type for active tracked feature --- ov_msckf/msg/ros2/OVActiveFeature.msg | 10 ++++++++++ ov_msckf/msg/ros2/OVActiveFeatureArray.msg | 7 +++++++ 2 files changed, 17 insertions(+) create mode 100644 ov_msckf/msg/ros2/OVActiveFeature.msg create mode 100644 ov_msckf/msg/ros2/OVActiveFeatureArray.msg diff --git a/ov_msckf/msg/ros2/OVActiveFeature.msg b/ov_msckf/msg/ros2/OVActiveFeature.msg new file mode 100644 index 000000000..70e9a84db --- /dev/null +++ b/ov_msckf/msg/ros2/OVActiveFeature.msg @@ -0,0 +1,10 @@ +# This message is created in publish_loopclosure_information for use in OVActiveFeatureArray.msg + +# Feature id +uint32 featid + +# Global position of feature +geometry_msgs/Vector3 posinglobal + +# uvd (uv coords and depth) of feature in current camera image +geometry_msgs/Vector3 uvd \ No newline at end of file diff --git a/ov_msckf/msg/ros2/OVActiveFeatureArray.msg b/ov_msckf/msg/ros2/OVActiveFeatureArray.msg new file mode 100644 index 000000000..7f4f802c3 --- /dev/null +++ b/ov_msckf/msg/ros2/OVActiveFeatureArray.msg @@ -0,0 +1,7 @@ +# This message is published in publish_loopclosure_information() function + +# Camera image timestamp +std_msgs/Header header + +# Array of active features in camera image +ov_msckf/OVActiveFeature[] data \ No newline at end of file From 2a018169fcc42263b81ee4e6a3c1f04368426a79 Mon Sep 17 00:00:00 2001 From: ori Date: Wed, 30 Jul 2025 14:09:18 +0100 Subject: [PATCH 05/11] add active feature tracking --- ov_msckf/cmake/ROS2.cmake | 2 + ov_msckf/src/core/VioManager.cpp | 4 +- ov_msckf/src/core/VioManager.h | 2 +- ov_msckf/src/core/VioManagerHelper.cpp | 3 +- ov_msckf/src/listen_runtime.cpp | 56 +++++++++++++++++++++++++- ov_msckf/src/ros/ROS2Visualizer.cpp | 44 ++++++++++++++++++-- ov_msckf/src/ros/ROS2Visualizer.h | 6 ++- 7 files changed, 107 insertions(+), 10 deletions(-) diff --git a/ov_msckf/cmake/ROS2.cmake b/ov_msckf/cmake/ROS2.cmake index 104ce8e5e..b9e7ff559 100644 --- a/ov_msckf/cmake/ROS2.cmake +++ b/ov_msckf/cmake/ROS2.cmake @@ -25,6 +25,8 @@ add_definitions(-DROS_AVAILABLE=2) # Add message files set(msg_files msg/ros2/OVRuntimeStatus.msg + msg/ros2/OVActiveFeature.msg + msg/ros2/OVActiveFeatureArray.msg ) rosidl_generate_interfaces(${PROJECT_NAME} diff --git a/ov_msckf/src/core/VioManager.cpp b/ov_msckf/src/core/VioManager.cpp index 6a0d7b914..c0602ce8a 100644 --- a/ov_msckf/src/core/VioManager.cpp +++ b/ov_msckf/src/core/VioManager.cpp @@ -522,7 +522,7 @@ void VioManager::do_feature_propagate_update(const ov_core::CameraData &message) // NOTE: this should only really be used if you want to track a lot of features, or have limited computational resources if ((int)featsup_MSCKF.size() > state->_options.max_msckf_in_update) featsup_MSCKF.erase(featsup_MSCKF.begin(), featsup_MSCKF.end() - state->_options.max_msckf_in_update); - updaterMSCKF->update(state, featsup_MSCKF); + updaterMSCKF->update(state, featsup_MSCKF); //==> CORE TODO: This method removes bad features, so sample from featsup_MSCKF after this line propagator->invalidate_cache(); rT4 = boost::posix_time::microsec_clock::local_time(); @@ -538,7 +538,7 @@ void VioManager::do_feature_propagate_update(const ov_core::CameraData &message) feats_slam_UPDATE.erase(feats_slam_UPDATE.begin(), feats_slam_UPDATE.begin() + std::min(state->_options.max_slam_in_update, (int)feats_slam_UPDATE.size())); // Do the update - updaterSLAM->update(state, featsup_TEMP); + updaterSLAM->update(state, featsup_TEMP); //==> CORE TODO: This method is for SLAM update, also removes bad features feats_slam_UPDATE_TEMP.insert(feats_slam_UPDATE_TEMP.end(), featsup_TEMP.begin(), featsup_TEMP.end()); propagator->invalidate_cache(); } diff --git a/ov_msckf/src/core/VioManager.h b/ov_msckf/src/core/VioManager.h index 3fd708901..0418b65bc 100644 --- a/ov_msckf/src/core/VioManager.h +++ b/ov_msckf/src/core/VioManager.h @@ -231,7 +231,7 @@ class VioManager { // Good features that where used in the last update (used in visualization) std::vector good_features_MSCKF; - + // Re-triangulated features 3d positions seen from the current frame (used in visualization) // For each feature we have a linear system A * p_FinG = b we create and increment their costs double active_tracks_time = -1; diff --git a/ov_msckf/src/core/VioManagerHelper.cpp b/ov_msckf/src/core/VioManagerHelper.cpp index 48198bc14..357224b95 100644 --- a/ov_msckf/src/core/VioManagerHelper.cpp +++ b/ov_msckf/src/core/VioManagerHelper.cpp @@ -375,7 +375,8 @@ void VioManager::retriangulate_active_tracks(const ov_core::CameraData &message) // Finally construct the uv and depth Eigen::Vector3d uvd; uvd << uv_dist, depth; - active_tracks_uvd.insert({feat.first, uvd}); + //PRINT_INFO(REDPURPLE "%d %f \n"RESET, feat.first, depth); + active_tracks_uvd.insert({feat.first, uvd}); // Get uv coordinates and depth of active tracked feature projected onto current frame } retri_rT3 = boost::posix_time::microsec_clock::local_time(); diff --git a/ov_msckf/src/listen_runtime.cpp b/ov_msckf/src/listen_runtime.cpp index a927104c4..5fcdfbac6 100644 --- a/ov_msckf/src/listen_runtime.cpp +++ b/ov_msckf/src/listen_runtime.cpp @@ -27,9 +27,12 @@ #include #include #include +#include #include "rclcpp/rclcpp.hpp" #include "ov_msckf/msg/ov_runtime_status.hpp" +#include "ov_msckf/msg/ov_active_feature.hpp" +#include "ov_msckf/msg/ov_active_feature_array.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" #include "utils/csvfile.h" @@ -45,6 +48,11 @@ class RuntimeSubscriber : public rclcpp::Node this->subOVRuntimeStatus_ = this->create_subscription( "/ov_msckf/runtime_status", 10, std::bind(&RuntimeSubscriber::topic1_callback, this, std::placeholders::_1) ); + + // Subscribe to OVActiveFeatureArray + this->subOVActiveFeatureArray_ = this->create_subscription( + "/ov_msckf/active_features", 10, std::bind(&RuntimeSubscriber::topic2_callback, this, std::placeholders::_1) + ); // Get save file path, create if it doesn't exist this->declare_parameter("save_path", "unset_save_path"); @@ -54,11 +62,13 @@ class RuntimeSubscriber : public rclcpp::Node std::string bias_path = this->save_path + "_bias.csv"; std::string msckf_path = this->save_path + "_msckf.csv"; std::string slam_path = this->save_path + "_slam.csv"; + std::string feat_path = this->save_path + "_feat.csv"; // Open csv files at save_path this->bias_csv = std::make_unique(bias_path); this->msckf_csv = std::make_unique(msckf_path); this->slam_csv = std::make_unique(slam_path); + this->feat_csv = std::make_unique(feat_path); RCLCPP_INFO(this->get_logger(), "Created .csv files"); // Populate first lines @@ -72,6 +82,12 @@ class RuntimeSubscriber : public rclcpp::Node *(this->slam_csv) << "sec" << "nanosec" << "frame_id" << "slam_num_points" << "x" << "y" << "z" << endrow; + + *(this->feat_csv) << "sec" << "nanosec" << "frame_id" << "feat_num" + << "featid" + << "posx" << "posy" << "posz" + << "u" << "v" << "d" + << "..." << endrow; } private: @@ -139,7 +155,7 @@ class RuntimeSubscriber : public rclcpp::Node // Save xyz to csv *(this->msckf_csv) << x_str << y_str << z_str; - RCLCPP_INFO(this->get_logger(), "Point %zu: x=%f, y=%f, z=%f", i, x, y, z); + //RCLCPP_INFO(this->get_logger(), "Point %zu: x=%f, y=%f, z=%f", i, x, y, z); } *(this->msckf_csv) << endrow; } @@ -186,18 +202,54 @@ class RuntimeSubscriber : public rclcpp::Node // Save xyz to csv *(this->slam_csv) << x_str << y_str << z_str; - RCLCPP_INFO(this->get_logger(), "Point %zu: x=%f, y=%f, z=%f", i, x, y, z); + //RCLCPP_INFO(this->get_logger(), "Point %zu: x=%f, y=%f, z=%f", i, x, y, z); } *(this->slam_csv) << endrow; } } } + + void topic2_callback(const ov_msckf::msg::OVActiveFeatureArray::SharedPtr feat_msg) { + + RCLCPP_INFO(this->get_logger(), "Received message on topic2"); + + std::string sec = std::to_string(feat_msg->header.stamp.sec); + std::string nanosec = std::to_string(feat_msg->header.stamp.nanosec); + std::string frame_id = feat_msg->header.frame_id; + *(this->feat_csv) << sec << nanosec << frame_id; + + std::vector feat_arr = feat_msg->data; + std::string feat_num = std::to_string(feat_arr.size()); + *(this->feat_csv) << feat_num; + + for (auto &feat : feat_arr) { + std::string tempStr; + tempStr = std::to_string(feat.featid); + *(this->feat_csv) << tempStr; + tempStr = std::to_string(feat.posinglobal.x); + *(this->feat_csv) << tempStr; + tempStr = std::to_string(feat.posinglobal.y); + *(this->feat_csv) << tempStr; + tempStr = std::to_string(feat.posinglobal.z); + *(this->feat_csv) << tempStr; + tempStr = std::to_string(feat.uvd.x); + *(this->feat_csv) << tempStr; + tempStr = std::to_string(feat.uvd.y); + *(this->feat_csv) << tempStr; + tempStr = std::to_string(feat.uvd.z); + *(this->feat_csv) << tempStr; + } + *(this->feat_csv) << endrow; + } rclcpp::Subscription::SharedPtr subOVRuntimeStatus_; + rclcpp::Subscription::SharedPtr subOVActiveFeatureArray_; + std::string save_path; std::unique_ptr bias_csv; std::unique_ptr msckf_csv; std::unique_ptr slam_csv; + std::unique_ptr feat_csv; }; int main(int argc, char **argv) { diff --git a/ov_msckf/src/ros/ROS2Visualizer.cpp b/ov_msckf/src/ros/ROS2Visualizer.cpp index e6bea5087..50cec912f 100644 --- a/ov_msckf/src/ros/ROS2Visualizer.cpp +++ b/ov_msckf/src/ros/ROS2Visualizer.cpp @@ -30,6 +30,9 @@ #include "utils/dataset_reader.h" #include "utils/print.h" #include "utils/sensor_data.h" +#include "feat/Feature.h" +#include +#include using namespace ov_core; using namespace ov_type; @@ -54,6 +57,8 @@ ROS2Visualizer::ROS2Visualizer(std::shared_ptr node, std::shared_p pub_status = node->create_publisher("runtime_status", 2); PRINT_DEBUG("Publishing: %s\n", pub_status->get_topic_name()); + pub_active_features = node->create_publisher("active_features", 2); + PRINT_DEBUG("Publishing: %s\n", pub_active_features->get_topic_name()); // 3D points publishing pub_points_msckf = node->create_publisher("points_msckf", 2); @@ -704,12 +709,24 @@ void ROS2Visualizer::publish_features() { status_msg.num_msckf_features = feats_msckf.size(); status_msg.num_slam_features = feats_slam.size(); - // Get feature coordinates (global) + // Get feature coordinates (global 3d coords) sensor_msgs::msg::PointCloud2 cloud_msckf = ROSVisualizerHelper::get_ros_pointcloud(_node, feats_msckf); status_msg.cloud_msckf_features = cloud_msckf; sensor_msgs::msg::PointCloud2 cloud_slam = ROSVisualizerHelper::get_ros_pointcloud(_node, feats_slam); status_msg.cloud_slam_features = cloud_slam; + // Get MSCKF feature UV + std::vector> good_features_MSCKF_full = _app->get_good_features_MSCKF_full(); + + for (auto &feat : good_features_MSCKF_full) { + for (auto &pair : feat->uvs) { + PRINT_INFO(REDPURPLE "%d, %d" RESET, pair.first, pair.second.size()); + for (auto &vec : pair.second) { + PRINT_INFO("\n %f, %f", vec[0], vec[1]); + } + } + } + // Publish runtime status pub_status->publish(status_msg); // PRINT_DEBUG("Published runtime status with %ld MSCKF features and %ld SLAM features\n", @@ -885,8 +902,8 @@ void ROS2Visualizer::publish_loopclosure_information() { // Get the current tracks in this frame double active_tracks_time1 = -1; double active_tracks_time2 = -1; - std::unordered_map active_tracks_posinG; - std::unordered_map active_tracks_uvd; + std::unordered_map active_tracks_posinG; // + std::unordered_map active_tracks_uvd; // cv::Mat active_cam0_image; _app->get_active_tracks(active_tracks_time1, active_tracks_posinG, active_tracks_uvd); _app->get_active_image(active_tracks_time2, active_cam0_image); @@ -896,6 +913,27 @@ void ROS2Visualizer::publish_loopclosure_information() { return; if (active_tracks_time1 != active_tracks_time2) return; + + // Get active tracked features + ov_msckf::msg::OVActiveFeatureArray featArray_msg; // Message for array of active features + featArray_msg.header.stamp = ROSVisualizerHelper::get_time_from_seconds(active_tracks_time1); // Timestamp of image + featArray_msg.header.frame_id = ""; + + // For every feature in current camera image + for (auto &pair : active_tracks_posinG) { + ov_msckf::msg::OVActiveFeature feat_msg; // Message for single feature (featid, posinG, uvd) + Eigen::Vector3d uvd = active_tracks_uvd[pair.first]; + + // Store featid, posinG (posinglobal), uvd of feature + feat_msg.featid = pair.first; + feat_msg.posinglobal = geometry_msgs::build().x(pair.second.x()).y(pair.second.y()).z(pair.second.z()); + feat_msg.uvd = geometry_msgs::build().x(uvd.x()).y(uvd.y()).z(uvd.z()); // Note uvd is given in frame of reference from cam0 + + // Push back feature msg into feature array msg + featArray_msg.data.push_back(feat_msg); + } + // Publish active features + pub_active_features->publish(featArray_msg); // Default header std_msgs::msg::Header header; diff --git a/ov_msckf/src/ros/ROS2Visualizer.h b/ov_msckf/src/ros/ROS2Visualizer.h index dbc90b41e..6ca97d2e2 100644 --- a/ov_msckf/src/ros/ROS2Visualizer.h +++ b/ov_msckf/src/ros/ROS2Visualizer.h @@ -25,6 +25,7 @@ #include #include #include +#include #include #include #include @@ -54,7 +55,9 @@ #include #include #include -#include +#include +#include +#include namespace ov_core { class YamlParser; @@ -155,6 +158,7 @@ class ROS2Visualizer { rclcpp::Publisher::SharedPtr pub_loop_point; rclcpp::Publisher::SharedPtr pub_loop_intrinsics; rclcpp::Publisher::SharedPtr pub_status; + rclcpp::Publisher::SharedPtr pub_active_features; std::shared_ptr mTfBr; // Our subscribers and camera synchronizers From 342a029d7aa6a4f28627c2cea52e213b7fd36fee Mon Sep 17 00:00:00 2001 From: ori Date: Wed, 30 Jul 2025 16:15:34 +0100 Subject: [PATCH 06/11] debugging; slam features are added to active features --- config/aria/estimator_config.yaml | 2 +- ov_msckf/src/core/VioManagerHelper.cpp | 4 ++++ ov_msckf/src/listen_runtime.cpp | 4 ++++ ov_msckf/src/ros/ROS2Visualizer.cpp | 23 +++++++---------------- 4 files changed, 16 insertions(+), 17 deletions(-) diff --git a/config/aria/estimator_config.yaml b/config/aria/estimator_config.yaml index 4cb5a9ab3..944062334 100644 --- a/config/aria/estimator_config.yaml +++ b/config/aria/estimator_config.yaml @@ -1,6 +1,6 @@ %YAML:1.0 # need to specify the file type at the top! -verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT +verbosity: "ERROR" # 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) diff --git a/ov_msckf/src/core/VioManagerHelper.cpp b/ov_msckf/src/core/VioManagerHelper.cpp index 357224b95..6a6196390 100644 --- a/ov_msckf/src/core/VioManagerHelper.cpp +++ b/ov_msckf/src/core/VioManagerHelper.cpp @@ -361,9 +361,12 @@ void VioManager::retriangulate_active_tracks(const ov_core::CameraData &message) // Skip if not valid (i.e. negative depth, or outside of image) if (depth < 0.1) { + //PRINT_INFO(REDPURPLE "SKIPPED %f, %f, %f\n" RESET, uv_dist(0), uv_dist(1), depth); continue; } + //std::cout << uv_dist(0) << ", " << uv_dist(1) << ", " << depth << std::endl; + // Skip if not valid (i.e. negative depth, or outside of image) int width = state->_cam_intrinsics_cameras.at(0)->w(); int height = state->_cam_intrinsics_cameras.at(0)->h(); @@ -378,6 +381,7 @@ void VioManager::retriangulate_active_tracks(const ov_core::CameraData &message) //PRINT_INFO(REDPURPLE "%d %f \n"RESET, feat.first, depth); active_tracks_uvd.insert({feat.first, uvd}); // Get uv coordinates and depth of active tracked feature projected onto current frame } + //std::cout << "BEFORE No Feature: " << active_tracks_uvd.size() << std::endl; retri_rT3 = boost::posix_time::microsec_clock::local_time(); // Timing information diff --git a/ov_msckf/src/listen_runtime.cpp b/ov_msckf/src/listen_runtime.cpp index 5fcdfbac6..1420ad9fb 100644 --- a/ov_msckf/src/listen_runtime.cpp +++ b/ov_msckf/src/listen_runtime.cpp @@ -218,6 +218,10 @@ class RuntimeSubscriber : public rclcpp::Node std::string frame_id = feat_msg->header.frame_id; *(this->feat_csv) << sec << nanosec << frame_id; + //std::string message = "Number of Data: "+std::to_string(feat_msg->data.size()); + //RCLCPP_INFO(this->get_logger(), "%s", message.c_str()); + //std::cout << "RECEIVED DATA: " << feat_msg->data.size() << std::endl; + std::vector feat_arr = feat_msg->data; std::string feat_num = std::to_string(feat_arr.size()); *(this->feat_csv) << feat_num; diff --git a/ov_msckf/src/ros/ROS2Visualizer.cpp b/ov_msckf/src/ros/ROS2Visualizer.cpp index 50cec912f..62ed81a69 100644 --- a/ov_msckf/src/ros/ROS2Visualizer.cpp +++ b/ov_msckf/src/ros/ROS2Visualizer.cpp @@ -714,18 +714,6 @@ void ROS2Visualizer::publish_features() { status_msg.cloud_msckf_features = cloud_msckf; sensor_msgs::msg::PointCloud2 cloud_slam = ROSVisualizerHelper::get_ros_pointcloud(_node, feats_slam); status_msg.cloud_slam_features = cloud_slam; - - // Get MSCKF feature UV - std::vector> good_features_MSCKF_full = _app->get_good_features_MSCKF_full(); - - for (auto &feat : good_features_MSCKF_full) { - for (auto &pair : feat->uvs) { - PRINT_INFO(REDPURPLE "%d, %d" RESET, pair.first, pair.second.size()); - for (auto &vec : pair.second) { - PRINT_INFO("\n %f, %f", vec[0], vec[1]); - } - } - } // Publish runtime status pub_status->publish(status_msg); @@ -907,6 +895,8 @@ void ROS2Visualizer::publish_loopclosure_information() { cv::Mat active_cam0_image; _app->get_active_tracks(active_tracks_time1, active_tracks_posinG, active_tracks_uvd); _app->get_active_image(active_tracks_time2, active_cam0_image); + //std::cout << "AFTER no of tracks: " << active_tracks_uvd.size() << std::endl; + //std::cout << "AFTER in posinG no of tracks: " << active_tracks_posinG.size() << std::endl; if (active_tracks_time1 == -1) return; if (_app->get_state()->_clones_IMU.find(active_tracks_time1) == _app->get_state()->_clones_IMU.end()) @@ -920,20 +910,21 @@ void ROS2Visualizer::publish_loopclosure_information() { featArray_msg.header.frame_id = ""; // For every feature in current camera image - for (auto &pair : active_tracks_posinG) { + for (auto &pair : active_tracks_uvd) { ov_msckf::msg::OVActiveFeature feat_msg; // Message for single feature (featid, posinG, uvd) - Eigen::Vector3d uvd = active_tracks_uvd[pair.first]; + Eigen::Vector3d xyz = active_tracks_posinG[pair.first]; // Store featid, posinG (posinglobal), uvd of feature feat_msg.featid = pair.first; - feat_msg.posinglobal = geometry_msgs::build().x(pair.second.x()).y(pair.second.y()).z(pair.second.z()); - feat_msg.uvd = geometry_msgs::build().x(uvd.x()).y(uvd.y()).z(uvd.z()); // Note uvd is given in frame of reference from cam0 + feat_msg.posinglobal = geometry_msgs::build().x(xyz.x()).y(xyz.y()).z(xyz.z()); + feat_msg.uvd = geometry_msgs::build().x(pair.second.x()).y(pair.second.y()).z(pair.second.z()); // Note uvd is given in frame of reference from cam0 // Push back feature msg into feature array msg featArray_msg.data.push_back(feat_msg); } // Publish active features pub_active_features->publish(featArray_msg); + //std::cout << "AGAIN AFTER no of tracks: " << featArray_msg.data.size() << std::endl; // Default header std_msgs::msg::Header header; From 6c8df03bfa792b4da74ffd922fb5f4714851c468 Mon Sep 17 00:00:00 2001 From: ori Date: Fri, 8 Aug 2025 17:27:46 +0100 Subject: [PATCH 07/11] get slam features in cam0 view for batch smoothing --- ov_msckf/msg/ros2/OVRuntimeStatus.msg | 5 +- ov_msckf/src/core/VioManager.h | 10 ++++ ov_msckf/src/core/VioManagerHelper.cpp | 40 +++++++++++++++- ov_msckf/src/listen_runtime.cpp | 65 ++++++++++++++++++++++++-- ov_msckf/src/ros/ROS2Visualizer.cpp | 41 +++++++++++++++- ov_msckf/src/ros/ROS2Visualizer.h | 1 + 6 files changed, 155 insertions(+), 7 deletions(-) diff --git a/ov_msckf/msg/ros2/OVRuntimeStatus.msg b/ov_msckf/msg/ros2/OVRuntimeStatus.msg index 8dbb0b40f..228271c35 100644 --- a/ov_msckf/msg/ros2/OVRuntimeStatus.msg +++ b/ov_msckf/msg/ros2/OVRuntimeStatus.msg @@ -17,4 +17,7 @@ float64 t_offset_imu_cam # Pointcloud for features (Eigen::Vector3d p_FinG; //=> position of feature F in the global frame G) sensor_msgs/PointCloud2 cloud_msckf_features -sensor_msgs/PointCloud2 cloud_slam_features \ No newline at end of file +sensor_msgs/PointCloud2 cloud_slam_features + +# Feature IDs for SLAM cloud +uint32[] slam_feat_ids \ No newline at end of file diff --git a/ov_msckf/src/core/VioManager.h b/ov_msckf/src/core/VioManager.h index 0418b65bc..4a8f7cc16 100644 --- a/ov_msckf/src/core/VioManager.h +++ b/ov_msckf/src/core/VioManager.h @@ -116,6 +116,9 @@ class VioManager { /// Returns 3d SLAM features in the global frame std::vector get_features_SLAM(); + /// CUSTOM: Returns 3d SLAM feature ids + std::vector get_feature_ids_SLAM(); + /// Returns 3d ARUCO features in the global frame std::vector get_features_ARUCO(); @@ -136,6 +139,11 @@ class VioManager { feat_tracks_uvd = active_tracks_uvd; } + void get_slam_tracks(double ×tamp, std::unordered_map &feat_slam_tracks_uvd) { + timestamp = active_tracks_time; + feat_slam_tracks_uvd = slam_tracks_uvd; + } + protected: /** * @brief Given a new set of camera images, this will track them. @@ -237,6 +245,8 @@ class VioManager { double active_tracks_time = -1; std::unordered_map active_tracks_posinG; std::unordered_map active_tracks_uvd; + std::unordered_map slam_tracks_uvd; + cv::Mat active_image; std::map active_feat_linsys_A; std::map active_feat_linsys_b; diff --git a/ov_msckf/src/core/VioManagerHelper.cpp b/ov_msckf/src/core/VioManagerHelper.cpp index 6a6196390..5d9d7a01c 100644 --- a/ov_msckf/src/core/VioManagerHelper.cpp +++ b/ov_msckf/src/core/VioManagerHelper.cpp @@ -203,6 +203,7 @@ void VioManager::retriangulate_active_tracks(const ov_core::CameraData &message) } active_tracks_posinG.clear(); active_tracks_uvd.clear(); + slam_tracks_uvd.clear(); // Current active tracks in our frontend // TODO: should probably assert here that these are at the message time... @@ -342,7 +343,12 @@ void VioManager::retriangulate_active_tracks(const ov_core::CameraData &message) // For now skip features not seen from current frame // TODO: should we publish other features not tracked in cam0?? - if (feat_uvs_in_cam0.find(feat.first) == feat_uvs_in_cam0.end()) + // if (feat_uvs_in_cam0.find(feat.first) == feat_uvs_in_cam0.end()) + // continue; + + // If neither a SLAM feature or in cam0 current frame, skip + bool is_slam_feature = (state->_features_SLAM.find(feat.first) != state->_features_SLAM.end()); + if (!is_slam_feature && feat_uvs_in_cam0.find(feat.first) == feat_uvs_in_cam0.end()) continue; // Calculate the depth of the feature in the current frame @@ -351,9 +357,11 @@ void VioManager::retriangulate_active_tracks(const ov_core::CameraData &message) Eigen::Vector3d p_FinCi = R_ItoC * p_FinIi + p_IinC; double depth = p_FinCi(2); Eigen::Vector2d uv_dist; + // If feature is in cam0 (should include SLAM features) if (feat_uvs_in_cam0.find(feat.first) != feat_uvs_in_cam0.end()) { uv_dist << (double)feat_uvs_in_cam0.at(feat.first).x, (double)feat_uvs_in_cam0.at(feat.first).y; } else { + // Then this feature must be a SLAM feature that isn't in cam0 Eigen::Vector2d uv_norm; uv_norm << p_FinCi(0) / depth, p_FinCi(1) / depth; uv_dist = state->_cam_intrinsics_cameras.at(0)->distort_d(uv_norm); @@ -380,6 +388,12 @@ void VioManager::retriangulate_active_tracks(const ov_core::CameraData &message) uvd << uv_dist, depth; //PRINT_INFO(REDPURPLE "%d %f \n"RESET, feat.first, depth); active_tracks_uvd.insert({feat.first, uvd}); // Get uv coordinates and depth of active tracked feature projected onto current frame + + // If feature is SLAM feature and is in cam0's view + if (is_slam_feature && feat_uvs_in_cam0.find(feat.first) == feat_uvs_in_cam0.end()) { + PRINT_DEBUG(REDPURPLE "INSERTED SLAM FEATURE\n" RESET); + slam_tracks_uvd.insert({feat.first, uvd}); + } } //std::cout << "BEFORE No Feature: " << active_tracks_uvd.size() << std::endl; retri_rT3 = boost::posix_time::microsec_clock::local_time(); @@ -442,6 +456,30 @@ std::vector VioManager::get_features_SLAM() { return slam_feats; } +// CUSTOM: clone of get_features_SLAM() returns feat id of slam landmark +std::vector VioManager::get_feature_ids_SLAM() { + std::vector slam_feats_id; + for (auto &f : state->_features_SLAM) { + if ((int)f.first <= 4 * state->_options.max_aruco_features) + continue; + if (ov_type::LandmarkRepresentation::is_relative_representation(f.second->_feat_representation)) { + // Assert that we have an anchor pose for this feature + assert(f.second->_anchor_cam_id != -1); + // // Get calibration for our anchor camera + // Eigen::Matrix R_ItoC = state->_calib_IMUtoCAM.at(f.second->_anchor_cam_id)->Rot(); + // Eigen::Matrix p_IinC = state->_calib_IMUtoCAM.at(f.second->_anchor_cam_id)->pos(); + // // Anchor pose orientation and position + // Eigen::Matrix R_GtoI = state->_clones_IMU.at(f.second->_anchor_clone_timestamp)->Rot(); + // Eigen::Matrix p_IinG = state->_clones_IMU.at(f.second->_anchor_clone_timestamp)->pos(); + // Feature in the global frame + slam_feats_id.push_back(f.first); + } else { + slam_feats_id.push_back(f.first); + } + } + return slam_feats_id; +} + std::vector VioManager::get_features_ARUCO() { std::vector aruco_feats; for (auto &f : state->_features_SLAM) { diff --git a/ov_msckf/src/listen_runtime.cpp b/ov_msckf/src/listen_runtime.cpp index 1420ad9fb..90f59713b 100644 --- a/ov_msckf/src/listen_runtime.cpp +++ b/ov_msckf/src/listen_runtime.cpp @@ -54,6 +54,11 @@ class RuntimeSubscriber : public rclcpp::Node "/ov_msckf/active_features", 10, std::bind(&RuntimeSubscriber::topic2_callback, this, std::placeholders::_1) ); + // Subscribe to SLAM features + this->subOVSlamFeatureArray_ = this->create_subscription( + "/ov_msckf/slam_features", 10, std::bind(&RuntimeSubscriber::topic3_callback, this, std::placeholders::_1) + ); + // Get save file path, create if it doesn't exist this->declare_parameter("save_path", "unset_save_path"); this->get_parameter("save_path", this->save_path); @@ -63,12 +68,14 @@ class RuntimeSubscriber : public rclcpp::Node std::string msckf_path = this->save_path + "_msckf.csv"; std::string slam_path = this->save_path + "_slam.csv"; std::string feat_path = this->save_path + "_feat.csv"; + std::string feat_SLAM_path = this->save_path + "_feat_SLAM.csv"; // Open csv files at save_path this->bias_csv = std::make_unique(bias_path); this->msckf_csv = std::make_unique(msckf_path); this->slam_csv = std::make_unique(slam_path); this->feat_csv = std::make_unique(feat_path); + this->feat_SLAM_csv = std::make_unique(feat_SLAM_path); RCLCPP_INFO(this->get_logger(), "Created .csv files"); // Populate first lines @@ -80,14 +87,21 @@ class RuntimeSubscriber : public rclcpp::Node *(this->msckf_csv) << "sec" << "nanosec" << "frame_id" << "msckf_num_points" << "x" << "y" << "z" << endrow; - *(this->slam_csv) << "sec" << "nanosec" << "frame_id" - << "slam_num_points" << "x" << "y" << "z" << endrow; + *(this->slam_csv) << "sec" << "nanosec" << "frame_id" << "slam_num_points" + << "featid" << "x" << "y" << "z" + << "..." << endrow; *(this->feat_csv) << "sec" << "nanosec" << "frame_id" << "feat_num" << "featid" << "posx" << "posy" << "posz" << "u" << "v" << "d" << "..." << endrow; + + *(this->feat_SLAM_csv) << "sec" << "nanosec" << "frame_id" << "feat_num" + << "featid" + << "posx" << "posy" << "posz" + << "u" << "v" << "d" + << "..." << endrow; } private: @@ -163,6 +177,7 @@ class RuntimeSubscriber : public rclcpp::Node // SLAM CSV sensor_msgs::msg::PointCloud2 cloud_slam = status_msg->cloud_slam_features; + std::vector slam_feat_ids = status_msg->slam_feat_ids; // Extract XYZ coordinates from cloud_slam if (cloud_slam.width * cloud_slam.height == 0) { @@ -186,11 +201,13 @@ class RuntimeSubscriber : public rclcpp::Node size_t num_points = cloud_slam.width * cloud_slam.height; *(this->slam_csv) << num_points; + std::string feat_id_str; std::string x_str; std::string y_str; std::string z_str; for (size_t i = 0; i < num_points; ++i) { + feat_id_str = std::to_string(slam_feat_ids[i]); const uint8_t* ptr = &cloud_slam.data[i * point_step]; float x = *reinterpret_cast(ptr + offset_x); float y = *reinterpret_cast(ptr + offset_y); @@ -201,7 +218,7 @@ class RuntimeSubscriber : public rclcpp::Node z_str = std::to_string(z); // Save xyz to csv - *(this->slam_csv) << x_str << y_str << z_str; + *(this->slam_csv) << feat_id_str << x_str << y_str << z_str; //RCLCPP_INFO(this->get_logger(), "Point %zu: x=%f, y=%f, z=%f", i, x, y, z); } *(this->slam_csv) << endrow; @@ -209,6 +226,7 @@ class RuntimeSubscriber : public rclcpp::Node } } + // For all features in cam0 void topic2_callback(const ov_msckf::msg::OVActiveFeatureArray::SharedPtr feat_msg) { RCLCPP_INFO(this->get_logger(), "Received message on topic2"); @@ -245,15 +263,56 @@ class RuntimeSubscriber : public rclcpp::Node } *(this->feat_csv) << endrow; } + + + // For SLAM features (can include those outside of cam0, should change source code to toggle: Search for TOGGLE in VioManagerHelper.cpp) + void topic3_callback(const ov_msckf::msg::OVActiveFeatureArray::SharedPtr feat_msg) { + + RCLCPP_INFO(this->get_logger(), "Received message on topic3"); + + std::string sec = std::to_string(feat_msg->header.stamp.sec); + std::string nanosec = std::to_string(feat_msg->header.stamp.nanosec); + std::string frame_id = feat_msg->header.frame_id; + *(this->feat_SLAM_csv) << sec << nanosec << frame_id; + + //std::string message = "Number of Data: "+std::to_string(feat_msg->data.size()); + //RCLCPP_INFO(this->get_logger(), "%s", message.c_str()); + //std::cout << "RECEIVED DATA: " << feat_msg->data.size() << std::endl; + + std::vector feat_arr = feat_msg->data; + std::string feat_num = std::to_string(feat_arr.size()); + *(this->feat_SLAM_csv) << feat_num; + + for (auto &feat : feat_arr) { + std::string tempStr; + tempStr = std::to_string(feat.featid); + *(this->feat_SLAM_csv) << tempStr; + tempStr = std::to_string(feat.posinglobal.x); + *(this->feat_SLAM_csv) << tempStr; + tempStr = std::to_string(feat.posinglobal.y); + *(this->feat_SLAM_csv) << tempStr; + tempStr = std::to_string(feat.posinglobal.z); + *(this->feat_SLAM_csv) << tempStr; + tempStr = std::to_string(feat.uvd.x); + *(this->feat_SLAM_csv) << tempStr; + tempStr = std::to_string(feat.uvd.y); + *(this->feat_SLAM_csv) << tempStr; + tempStr = std::to_string(feat.uvd.z); + *(this->feat_SLAM_csv) << tempStr; + } + *(this->feat_SLAM_csv) << endrow; + } rclcpp::Subscription::SharedPtr subOVRuntimeStatus_; rclcpp::Subscription::SharedPtr subOVActiveFeatureArray_; + rclcpp::Subscription::SharedPtr subOVSlamFeatureArray_; std::string save_path; std::unique_ptr bias_csv; std::unique_ptr msckf_csv; std::unique_ptr slam_csv; std::unique_ptr feat_csv; + std::unique_ptr feat_SLAM_csv; }; int main(int argc, char **argv) { diff --git a/ov_msckf/src/ros/ROS2Visualizer.cpp b/ov_msckf/src/ros/ROS2Visualizer.cpp index 62ed81a69..c1f063063 100644 --- a/ov_msckf/src/ros/ROS2Visualizer.cpp +++ b/ov_msckf/src/ros/ROS2Visualizer.cpp @@ -59,6 +59,8 @@ ROS2Visualizer::ROS2Visualizer(std::shared_ptr node, std::shared_p PRINT_DEBUG("Publishing: %s\n", pub_status->get_topic_name()); pub_active_features = node->create_publisher("active_features", 2); PRINT_DEBUG("Publishing: %s\n", pub_active_features->get_topic_name()); + pub_slam_features = node->create_publisher("slam_features", 2); + PRINT_DEBUG("Publishing: %s\n", pub_slam_features->get_topic_name()); // 3D points publishing pub_points_msckf = node->create_publisher("points_msckf", 2); @@ -706,6 +708,8 @@ void ROS2Visualizer::publish_features() { // Get feature counts std::vector feats_msckf = _app->get_good_features_MSCKF(); std::vector feats_slam = _app->get_features_SLAM(); + std::vector slam_feat_ids = _app->get_feature_ids_SLAM(); + status_msg.num_msckf_features = feats_msckf.size(); status_msg.num_slam_features = feats_slam.size(); @@ -714,7 +718,13 @@ void ROS2Visualizer::publish_features() { status_msg.cloud_msckf_features = cloud_msckf; sensor_msgs::msg::PointCloud2 cloud_slam = ROSVisualizerHelper::get_ros_pointcloud(_node, feats_slam); status_msg.cloud_slam_features = cloud_slam; - + + // Populate SLAM feature IDs (convert from size_t to uint32_t) + status_msg.slam_feat_ids.resize(slam_feat_ids.size()); + for (size_t i = 0; i < slam_feat_ids.size(); i++) { + status_msg.slam_feat_ids[i] = static_cast(slam_feat_ids[i]); + } + // Publish runtime status pub_status->publish(status_msg); // PRINT_DEBUG("Published runtime status with %ld MSCKF features and %ld SLAM features\n", @@ -890,10 +900,13 @@ void ROS2Visualizer::publish_loopclosure_information() { // Get the current tracks in this frame double active_tracks_time1 = -1; double active_tracks_time2 = -1; + double active_tracks_time3 = -1; std::unordered_map active_tracks_posinG; // std::unordered_map active_tracks_uvd; // + std::unordered_map slam_tracks_uvd; // of SLAM features not seen in cam0 cv::Mat active_cam0_image; _app->get_active_tracks(active_tracks_time1, active_tracks_posinG, active_tracks_uvd); + _app->get_slam_tracks(active_tracks_time2, slam_tracks_uvd); _app->get_active_image(active_tracks_time2, active_cam0_image); //std::cout << "AFTER no of tracks: " << active_tracks_uvd.size() << std::endl; //std::cout << "AFTER in posinG no of tracks: " << active_tracks_posinG.size() << std::endl; @@ -907,7 +920,7 @@ void ROS2Visualizer::publish_loopclosure_information() { // Get active tracked features ov_msckf::msg::OVActiveFeatureArray featArray_msg; // Message for array of active features featArray_msg.header.stamp = ROSVisualizerHelper::get_time_from_seconds(active_tracks_time1); // Timestamp of image - featArray_msg.header.frame_id = ""; + featArray_msg.header.frame_id = "cam0"; // For every feature in current camera image for (auto &pair : active_tracks_uvd) { @@ -922,8 +935,32 @@ void ROS2Visualizer::publish_loopclosure_information() { // Push back feature msg into feature array msg featArray_msg.data.push_back(feat_msg); } + + // Get SLAM tracked features + ov_msckf::msg::OVActiveFeatureArray slamFeatArray_msg; // Message for array of active features + slamFeatArray_msg.header.stamp = ROSVisualizerHelper::get_time_from_seconds(active_tracks_time1); // Timestamp of image + slamFeatArray_msg.header.frame_id = "cam0"; + + // For every SLAM feature not in current camera image + for (auto &pair : slam_tracks_uvd) { + //PRINT_INFO(REDPURPLE"Added SLAM feature to message#n"RESET); + ov_msckf::msg::OVActiveFeature feat_msg; // Message for single feature (featid, posinG, uvd) + Eigen::Vector3d xyz = active_tracks_posinG[pair.first]; + + // Store featid, posinG (posinglobal), uvd of feature + feat_msg.featid = pair.first; + feat_msg.posinglobal = geometry_msgs::build().x(xyz.x()).y(xyz.y()).z(xyz.z()); + feat_msg.uvd = geometry_msgs::build().x(pair.second.x()).y(pair.second.y()).z(pair.second.z()); // Note uvd is given in frame of reference from cam0 + + // Push back feature msg into feature array msg + slamFeatArray_msg.data.push_back(feat_msg); + } + // Publish active features pub_active_features->publish(featArray_msg); + + // Publish SLAM not in cam0 features + pub_slam_features->publish(slamFeatArray_msg); //std::cout << "AGAIN AFTER no of tracks: " << featArray_msg.data.size() << std::endl; // Default header diff --git a/ov_msckf/src/ros/ROS2Visualizer.h b/ov_msckf/src/ros/ROS2Visualizer.h index 6ca97d2e2..361fbd8d1 100644 --- a/ov_msckf/src/ros/ROS2Visualizer.h +++ b/ov_msckf/src/ros/ROS2Visualizer.h @@ -159,6 +159,7 @@ class ROS2Visualizer { rclcpp::Publisher::SharedPtr pub_loop_intrinsics; rclcpp::Publisher::SharedPtr pub_status; rclcpp::Publisher::SharedPtr pub_active_features; + rclcpp::Publisher::SharedPtr pub_slam_features; std::shared_ptr mTfBr; // Our subscribers and camera synchronizers From ee5d6531c07153f9c95cb143f7563b640102b0cd Mon Sep 17 00:00:00 2001 From: Jin Rhee Date: Fri, 12 Sep 2025 15:22:31 +0100 Subject: [PATCH 08/11] add time offset and listener-only script --- ov_msckf/launch/aria_listener_only.launch.py | 128 +++++++++++++++++++ ov_msckf/msg/ros2/OVActiveFeatureArray.msg | 5 +- ov_msckf/src/listen_runtime.cpp | 8 +- ov_msckf/src/ros/ROS2Visualizer.cpp | 23 +++- 4 files changed, 157 insertions(+), 7 deletions(-) create mode 100644 ov_msckf/launch/aria_listener_only.launch.py diff --git a/ov_msckf/launch/aria_listener_only.launch.py b/ov_msckf/launch/aria_listener_only.launch.py new file mode 100644 index 000000000..94358c3f1 --- /dev/null +++ b/ov_msckf/launch/aria_listener_only.launch.py @@ -0,0 +1,128 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, TextSubstitution +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory, get_package_prefix +import os +import sys + +launch_args = [ + DeclareLaunchArgument(name="namespace", default_value="ov_msckf", description="namespace"), + DeclareLaunchArgument( + name="ov_enable", default_value="true", description="enable OpenVINS node" + ), + DeclareLaunchArgument( + name="rviz_enable", default_value="false", description="enable rviz node" + ), + DeclareLaunchArgument( + name="config", + default_value="aria", + description="euroc_mav, tum_vi, rpng_aruco...", + ), + DeclareLaunchArgument( + name="config_path", + default_value="", + description="path to estimator_config.yaml. If not given, determined based on provided 'config' above", + ), + DeclareLaunchArgument( + name="verbosity", + default_value="INFO", + description="ALL, DEBUG, INFO, WARNING, ERROR, SILENT", + ), + DeclareLaunchArgument( + name="use_stereo", + default_value="false", + description="if we have more than 1 camera, if we should try to track stereo constraints between pairs", + ), + DeclareLaunchArgument( + name="max_cameras", + default_value="2", + description="how many cameras we have 1 = mono, 2 = stereo, >2 = binocular (all mono tracking)", + ), + DeclareLaunchArgument( + name="save_total_state", + default_value="false", + description="record the total state with calibration and features to a txt file", + ), + DeclareLaunchArgument( + name="save_path", + default_value="aria", + description="save path for csv files" + ) +] + +def launch_setup(context): + config_path = LaunchConfiguration("config_path").perform(context) + if not config_path: + configs_dir = os.path.join(get_package_share_directory("ov_msckf"), "config/") + available_configs = os.listdir(configs_dir) + config = LaunchConfiguration("config").perform(context) + if config in available_configs: + config_path = os.path.join( + get_package_share_directory("ov_msckf"), + "config",config,"estimator_config.yaml" + ) + else: + return [ + LogInfo( + msg="ERROR: unknown config: '{}' - Available configs are: {} - not starting OpenVINS".format( + config, ", ".join(available_configs) + ) + ) + ] + else: + if not os.path.isfile(config_path): + return [ + LogInfo( + msg="ERROR: config_path file: '{}' - does not exist. - not starting OpenVINS".format( + config_path) + ) + ] + ''' + node1 = Node( + package="ov_msckf", + executable="run_subscribe_msckf", + condition=IfCondition(LaunchConfiguration("ov_enable")), + namespace=LaunchConfiguration("namespace"), + output='screen', + parameters=[ + {"verbosity": LaunchConfiguration("verbosity")}, + {"use_stereo": LaunchConfiguration("use_stereo")}, + {"max_cameras": LaunchConfiguration("max_cameras")}, + {"save_total_state": LaunchConfiguration("save_total_state")}, + {"config_path": config_path}, + ], + ) + + node2 = Node( + package="rviz2", + executable="rviz2", + condition=IfCondition(LaunchConfiguration("rviz_enable")), + arguments=[ + "-d" + + os.path.join( + get_package_share_directory("ov_msckf"), "launch", "display_ros2.rviz" + ), + "--ros-args", + "--log-level", + "warn", + ], + ) +''' + node3 = Node( + package="ov_msckf", + executable="listen_runtime", + parameters=[ + {"save_path": LaunchConfiguration("save_path")} + ] + ) + + return [node3] + + +def generate_launch_description(): + opfunc = OpaqueFunction(function=launch_setup) + ld = LaunchDescription(launch_args) + ld.add_action(opfunc) + return ld diff --git a/ov_msckf/msg/ros2/OVActiveFeatureArray.msg b/ov_msckf/msg/ros2/OVActiveFeatureArray.msg index 7f4f802c3..dd19ce37e 100644 --- a/ov_msckf/msg/ros2/OVActiveFeatureArray.msg +++ b/ov_msckf/msg/ros2/OVActiveFeatureArray.msg @@ -4,4 +4,7 @@ std_msgs/Header header # Array of active features in camera image -ov_msckf/OVActiveFeature[] data \ No newline at end of file +ov_msckf/OVActiveFeature[] data + +# Time offset base IMU to camera (t_imu = t_cam + t_off) +float64 t_offset_imu_cam diff --git a/ov_msckf/src/listen_runtime.cpp b/ov_msckf/src/listen_runtime.cpp index 90f59713b..7d7d5eda5 100644 --- a/ov_msckf/src/listen_runtime.cpp +++ b/ov_msckf/src/listen_runtime.cpp @@ -91,13 +91,13 @@ class RuntimeSubscriber : public rclcpp::Node << "featid" << "x" << "y" << "z" << "..." << endrow; - *(this->feat_csv) << "sec" << "nanosec" << "frame_id" << "feat_num" + *(this->feat_csv) << "sec" << "nanosec" << "frame_id" << "feat_num" << "t_offset_imu_cam" << "featid" << "posx" << "posy" << "posz" << "u" << "v" << "d" << "..." << endrow; - *(this->feat_SLAM_csv) << "sec" << "nanosec" << "frame_id" << "feat_num" + *(this->feat_SLAM_csv) << "sec" << "nanosec" << "frame_id" << "feat_num" << "t_offset_imu_cam" << "featid" << "posx" << "posy" << "posz" << "u" << "v" << "d" @@ -243,6 +243,8 @@ class RuntimeSubscriber : public rclcpp::Node std::vector feat_arr = feat_msg->data; std::string feat_num = std::to_string(feat_arr.size()); *(this->feat_csv) << feat_num; + std::string t_offset_imu_cam = std::to_string(feat_msg->t_offset_imu_cam); + *(this->feat_csv) << t_offset_imu_cam; for (auto &feat : feat_arr) { std::string tempStr; @@ -282,6 +284,8 @@ class RuntimeSubscriber : public rclcpp::Node std::vector feat_arr = feat_msg->data; std::string feat_num = std::to_string(feat_arr.size()); *(this->feat_SLAM_csv) << feat_num; + std::string t_offset_imu_cam = std::to_string(feat_msg->t_offset_imu_cam); + *(this->feat_SLAM_csv) << t_offset_imu_cam; for (auto &feat : feat_arr) { std::string tempStr; diff --git a/ov_msckf/src/ros/ROS2Visualizer.cpp b/ov_msckf/src/ros/ROS2Visualizer.cpp index c1f063063..411401f75 100644 --- a/ov_msckf/src/ros/ROS2Visualizer.cpp +++ b/ov_msckf/src/ros/ROS2Visualizer.cpp @@ -135,8 +135,14 @@ ROS2Visualizer::ROS2Visualizer(std::shared_ptr node, std::shared_p boost::filesystem::remove(filepath_std); // Create folder path to this location if not exists - boost::filesystem::create_directories(boost::filesystem::path(filepath_est.c_str()).parent_path()); - boost::filesystem::create_directories(boost::filesystem::path(filepath_std.c_str()).parent_path()); + auto parent_path_est = boost::filesystem::path(filepath_est.c_str()).parent_path(); + if (!parent_path_est.empty()) { + boost::filesystem::create_directories(parent_path_est); + } + auto parent_path_std = boost::filesystem::path(filepath_std.c_str()).parent_path(); + if (!parent_path_std.empty()) { + boost::filesystem::create_directories(parent_path_std); + } // Open the files of_state_est.open(filepath_est.c_str()); @@ -150,7 +156,10 @@ ROS2Visualizer::ROS2Visualizer(std::shared_ptr node, std::shared_p if (_sim != nullptr) { if (boost::filesystem::exists(filepath_gt)) boost::filesystem::remove(filepath_gt); - boost::filesystem::create_directories(boost::filesystem::path(filepath_gt.c_str()).parent_path()); + auto parent_path_gt = boost::filesystem::path(filepath_gt.c_str()).parent_path(); + if (!parent_path_gt.empty()) { + boost::filesystem::create_directories(parent_path_gt); + } of_state_gt.open(filepath_gt.c_str()); of_state_gt << "# timestamp(s) q p v bg ba cam_imu_dt num_cam cam0_k cam0_d cam0_rot cam0_trans ... imu_model dw da tg wtoI atoI etc" << std::endl; @@ -921,7 +930,7 @@ void ROS2Visualizer::publish_loopclosure_information() { ov_msckf::msg::OVActiveFeatureArray featArray_msg; // Message for array of active features featArray_msg.header.stamp = ROSVisualizerHelper::get_time_from_seconds(active_tracks_time1); // Timestamp of image featArray_msg.header.frame_id = "cam0"; - + // For every feature in current camera image for (auto &pair : active_tracks_uvd) { ov_msckf::msg::OVActiveFeature feat_msg; // Message for single feature (featid, posinG, uvd) @@ -936,6 +945,9 @@ void ROS2Visualizer::publish_loopclosure_information() { featArray_msg.data.push_back(feat_msg); } + // Get cam-imu time offset + featArray_msg.t_offset_imu_cam = _app->get_state()->_calib_dt_CAMtoIMU->value()(0); + // Get SLAM tracked features ov_msckf::msg::OVActiveFeatureArray slamFeatArray_msg; // Message for array of active features slamFeatArray_msg.header.stamp = ROSVisualizerHelper::get_time_from_seconds(active_tracks_time1); // Timestamp of image @@ -956,6 +968,9 @@ void ROS2Visualizer::publish_loopclosure_information() { slamFeatArray_msg.data.push_back(feat_msg); } + // Get cam-imu time offset + slamFeatArray_msg.t_offset_imu_cam = _app->get_state()->_calib_dt_CAMtoIMU->value()(0); + // Publish active features pub_active_features->publish(featArray_msg); From 11bc1bb55afad8a0416f2ce0d323d559f72b0de2 Mon Sep 17 00:00:00 2001 From: Jin Rhee Date: Fri, 10 Oct 2025 16:49:08 +0100 Subject: [PATCH 09/11] add camera_rgb_righted to calibration files --- config/aria/kalibr_imucam_chain.yaml | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/config/aria/kalibr_imucam_chain.yaml b/config/aria/kalibr_imucam_chain.yaml index d6443cc50..e467b9184 100644 --- a/config/aria/kalibr_imucam_chain.yaml +++ b/config/aria/kalibr_imucam_chain.yaml @@ -1,4 +1,7 @@ %YAML:1.0 +# cam0: camera_slam_left +# cam1: camera_slam_right +# cam2: camera_rgb cam0: T_cam_imu: @@ -27,4 +30,18 @@ cam1: intrinsics: [242.1538223221408, 242.14114214084816, 317.8751234840723, 244.03555111568332] resolution: [640, 480] rostopic: /camera_slam_right/image - timeshift_cam_imu: 0.004075260451014888 \ No newline at end of file + timeshift_cam_imu: 0.004075260451014888 +cam2: + T_cam_imu: + - [-0.1061564415671857, 0.7958374298669715, -0.5961321960241086, 0.1258230800185664] + - [0.9936801665484081, 0.06291350562179068, -0.09296030022921153, 0.005361067249285295] + - [-0.03647652014818453, -0.6022330745094548, -0.7974865437389955, -0.016394991903615534] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [] + camera_model: pinhole + distortion_coeffs: [0.205362163753855, 0.8177824523079911, -1.4284013566280807, 1.07228296107305] + distortion_model: equidistant + intrinsics: [609.2948192551593, 608.364081254451, 703.7315385970343, 702.9153221781557] + resolution: [1408, 1408] + rostopic: /camera_rgb_righted/image + timeshift_cam_imu: 0.006857702469605033 \ No newline at end of file From d647b0f0d47c8425a324d249ff7ffc318269a0ba Mon Sep 17 00:00:00 2001 From: Jin Rhee Date: Tue, 4 Nov 2025 15:32:57 +0000 Subject: [PATCH 10/11] fixed yaml indentation --- config/aria/kalibr_imucam_chain.yaml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/config/aria/kalibr_imucam_chain.yaml b/config/aria/kalibr_imucam_chain.yaml index e467b9184..35d3d14e7 100644 --- a/config/aria/kalibr_imucam_chain.yaml +++ b/config/aria/kalibr_imucam_chain.yaml @@ -33,10 +33,10 @@ cam1: timeshift_cam_imu: 0.004075260451014888 cam2: T_cam_imu: - - [-0.1061564415671857, 0.7958374298669715, -0.5961321960241086, 0.1258230800185664] - - [0.9936801665484081, 0.06291350562179068, -0.09296030022921153, 0.005361067249285295] - - [-0.03647652014818453, -0.6022330745094548, -0.7974865437389955, -0.016394991903615534] - - [0.0, 0.0, 0.0, 1.0] + - [-0.1061564415671857, 0.7958374298669715, -0.5961321960241086, 0.1258230800185664] + - [0.9936801665484081, 0.06291350562179068, -0.09296030022921153, 0.005361067249285295] + - [-0.03647652014818453, -0.6022330745094548, -0.7974865437389955, -0.016394991903615534] + - [0.0, 0.0, 0.0, 1.0] cam_overlaps: [] camera_model: pinhole distortion_coeffs: [0.205362163753855, 0.8177824523079911, -1.4284013566280807, 1.07228296107305] From 7651e7bc8604fa8d41ef308388779057c79b61e0 Mon Sep 17 00:00:00 2001 From: Jin Rhee Date: Tue, 4 Nov 2025 16:21:02 +0000 Subject: [PATCH 11/11] update camera_rgb_righted description --- config/aria/kalibr_imucam_chain.yaml | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/config/aria/kalibr_imucam_chain.yaml b/config/aria/kalibr_imucam_chain.yaml index 35d3d14e7..46ab7af15 100644 --- a/config/aria/kalibr_imucam_chain.yaml +++ b/config/aria/kalibr_imucam_chain.yaml @@ -1,7 +1,9 @@ %YAML:1.0 # cam0: camera_slam_left # cam1: camera_slam_right -# cam2: camera_rgb +# cam2: camera_rgb # This topic should really be called /camera_rgb_righted/image, as the image + # information (and the extrinsics here) are for images that have been righted + # (i.e. rotated by 90 degrees from the Aria glass' raw image) cam0: T_cam_imu: @@ -43,5 +45,5 @@ cam2: distortion_model: equidistant intrinsics: [609.2948192551593, 608.364081254451, 703.7315385970343, 702.9153221781557] resolution: [1408, 1408] - rostopic: /camera_rgb_righted/image + rostopic: /camera_rgb/image timeshift_cam_imu: 0.006857702469605033 \ No newline at end of file