diff --git a/config/viode_config/estimator_config.yaml b/config/viode_config/estimator_config.yaml new file mode 100644 index 000000000..b2b242a89 --- /dev/null +++ b/config/viode_config/estimator_config.yaml @@ -0,0 +1,124 @@ +%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: 2 # how many cameras we have 1 = mono, 2 = stereo, >2 = binocular (all mono tracking) + +calib_cam_extrinsics: false # if the transform between camera and IMU should be optimized R_ItoC, p_CinI +calib_cam_intrinsics: false # if camera intrinsics should be optimized (focal, center, distortion) +calib_cam_timeoffset: false # 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: 50 # 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: 40 # how many MSCKF features to use in the update +dt_slam_delay: 1 # delay before initializing (helps with stability from bad initialization...) + +gravity_mag: 9.81007 # magnitude of gravity in this location + +feat_rep_msckf: "GLOBAL_3D" +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 # set to 0 for only disp-based +zupt_max_velocity: 0.1 +zupt_noise_multiplier: 10 +zupt_max_disparity: 0.5 # set to 0 for only imu-based +zupt_only_at_beginning: false + +# ================================================================== +# ================================================================== + +init_window_time: 1.2 # 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: 20.0 # max disparity to consider the platform stationary (dependent on resolution) +init_max_features: 75 # how many features to track during initialization (saves on computation) + +init_dyn_use: true # 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: 5.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: 600 # number of points (per camera) we will extract and try to track +fast_threshold: 10 # threshold for fast extraction (warning: lower threshs can be expensive) +grid_x: 6 # extraction sub-grid count for horizontal direction (uniform tracking) +grid_y: 4 # extraction sub-grid count for vertical direction (uniform tracking) +min_px_dist: 15 # distance between features (features near each other provide less information) +knn_ratio: 0.90 # descriptor knn threshold for the top two descriptor matches +track_frequency: 21.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: 1 # -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.5 +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 +# No need when use mask is false +mask0: "dummy_mask.png" +mask1: "dummy_mask.png" + +# 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" + +# Custom Dynamic Mask +use_dynamic_mask: true +dynamic_mask_topic0: "/cam0/masked" +dynamic_mask_topic1: "/cam1/masked" + diff --git a/config/viode_config/kalibr_imu_chain.yaml b/config/viode_config/kalibr_imu_chain.yaml new file mode 100644 index 000000000..8be418e67 --- /dev/null +++ b/config/viode_config/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.2 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) + accelerometer_random_walk: 0.02 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) + gyroscope_noise_density: 0.05 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) + gyroscope_random_walk: 4.0e-5 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) + rostopic: /imu0 + time_offset: 0.0 + update_rate: 200.0 + # 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/viode_config/kalibr_imucam_chain.yaml b/config/viode_config/kalibr_imucam_chain.yaml new file mode 100644 index 000000000..3f904d090 --- /dev/null +++ b/config/viode_config/kalibr_imucam_chain.yaml @@ -0,0 +1,28 @@ +%YAML:1.0 + +cam0: + T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI + - [0.0, 0.0, 1.0, 0.0] + - [1.0, 0.0, 0.0, 0.0] + - [0.0, 1.0, 0.0, 0.0] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [1] + camera_model: pinhole + distortion_coeffs: [0.0, 0.0, 0.0, 0.0] + distortion_model: radtan + intrinsics: [376.0, 376.0, 376.0, 240.0] #fu, fv, cu, cv + resolution: [752, 480] + rostopic: /cam0/image_raw +cam1: + T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI + - [0.0, 0.0, 1.0, 0.0] + - [1.0, 0.0, 0.0, 0.05] + - [0.0, 1.0, 0.0, 0.0] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [0] + camera_model: pinhole + distortion_coeffs: [0.0, 0.0, 0.0, 0.0] + distortion_model: radtan + intrinsics: [376.0, 376.0, 376.0, 240.0] #fu, fv, cu, cv + resolution: [752, 480] + rostopic: /cam1/image_raw diff --git a/ov_core/src/plot/matplotlibcpp.h b/ov_core/src/plot/matplotlibcpp.h index dd8be4e2d..fe6fb2a29 100644 --- a/ov_core/src/plot/matplotlibcpp.h +++ b/ov_core/src/plot/matplotlibcpp.h @@ -257,18 +257,42 @@ inline bool annotate(std::string annotation, double x, double y) { #ifndef WITHOUT_NUMPY // Type selector for numpy array conversion -template struct select_npy_type { const static NPY_TYPES type = NPY_NOTYPE; }; // Default -template <> struct select_npy_type { const static NPY_TYPES type = NPY_DOUBLE; }; -template <> struct select_npy_type { const static NPY_TYPES type = NPY_FLOAT; }; -template <> struct select_npy_type { const static NPY_TYPES type = NPY_BOOL; }; -template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT8; }; -template <> struct select_npy_type { const static NPY_TYPES type = NPY_SHORT; }; -template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT; }; -template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT64; }; -template <> struct select_npy_type { const static NPY_TYPES type = NPY_UINT8; }; -template <> struct select_npy_type { const static NPY_TYPES type = NPY_USHORT; }; -template <> struct select_npy_type { const static NPY_TYPES type = NPY_ULONG; }; -template <> struct select_npy_type { const static NPY_TYPES type = NPY_UINT64; }; +template struct select_npy_type { + const static NPY_TYPES type = NPY_NOTYPE; +}; // Default +template <> struct select_npy_type { + const static NPY_TYPES type = NPY_DOUBLE; +}; +template <> struct select_npy_type { + const static NPY_TYPES type = NPY_FLOAT; +}; +template <> struct select_npy_type { + const static NPY_TYPES type = NPY_BOOL; +}; +template <> struct select_npy_type { + const static NPY_TYPES type = NPY_INT8; +}; +template <> struct select_npy_type { + const static NPY_TYPES type = NPY_SHORT; +}; +template <> struct select_npy_type { + const static NPY_TYPES type = NPY_INT; +}; +template <> struct select_npy_type { + const static NPY_TYPES type = NPY_INT64; +}; +template <> struct select_npy_type { + const static NPY_TYPES type = NPY_UINT8; +}; +template <> struct select_npy_type { + const static NPY_TYPES type = NPY_USHORT; +}; +template <> struct select_npy_type { + const static NPY_TYPES type = NPY_ULONG; +}; +template <> struct select_npy_type { + const static NPY_TYPES type = NPY_UINT64; +}; template PyObject *get_array(const std::vector &v) { detail::_interpreter::get(); // interpreter needs to be initialized for the numpy commands to work @@ -1659,7 +1683,9 @@ template using is_function = typename std::is_function struct is_callable_impl; -template struct is_callable_impl { typedef is_function type; }; // a non-object is callable iff it is a function +template struct is_callable_impl { + typedef is_function type; +}; // a non-object is callable iff it is a function template struct is_callable_impl { struct Fallback { diff --git a/ov_core/src/track/TrackAruco.cpp b/ov_core/src/track/TrackAruco.cpp index 953eaa321..4d4c173ca 100644 --- a/ov_core/src/track/TrackAruco.cpp +++ b/ov_core/src/track/TrackAruco.cpp @@ -94,7 +94,7 @@ void TrackAruco::perform_tracking(double timestamp, const cv::Mat &imgin, size_t //=================================================================================== // Perform extraction -#if CV_MAJOR_VERSION > 4 || ( CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7) +#if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7) aruco_detector.detectMarkers(img0, corners[cam_id], ids_aruco[cam_id], rejects[cam_id]); #else cv::aruco::detectMarkers(img0, aruco_dict, corners[cam_id], ids_aruco[cam_id], aruco_params, rejects[cam_id]); diff --git a/ov_core/src/track/TrackAruco.h b/ov_core/src/track/TrackAruco.h index 35738aaae..61219466a 100644 --- a/ov_core/src/track/TrackAruco.h +++ b/ov_core/src/track/TrackAruco.h @@ -55,7 +55,7 @@ class TrackAruco : public TrackBase { bool downsize) : TrackBase(cameras, 0, numaruco, stereo, histmethod), max_tag_id(numaruco), do_downsizing(downsize) { #if ENABLE_ARUCO_TAGS -#if CV_MAJOR_VERSION > 4 || ( CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7) +#if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7) aruco_dict = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_1000); aruco_params.cornerRefinementMethod = cv::aruco::CORNER_REFINE_SUBPIX; aruco_detector = cv::aruco::ArucoDetector(aruco_dict, aruco_params); @@ -107,7 +107,7 @@ class TrackAruco : public TrackBase { bool do_downsizing; #if ENABLE_ARUCO_TAGS -#if CV_MAJOR_VERSION > 4 || ( CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7) +#if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7) // Our dictionary that we will extract aruco tags with cv::aruco::Dictionary aruco_dict; // Parameters the opencv extractor uses @@ -121,7 +121,6 @@ class TrackAruco : public TrackBase { cv::Ptr aruco_params; #endif - // Our tag IDs and corner we will get from the extractor std::unordered_map> ids_aruco; std::unordered_map>> corners, rejects; diff --git a/ov_init/src/ceres/State_JPLQuatLocal.h b/ov_init/src/ceres/State_JPLQuatLocal.h index 2df21467f..b57b39c1f 100644 --- a/ov_init/src/ceres/State_JPLQuatLocal.h +++ b/ov_init/src/ceres/State_JPLQuatLocal.h @@ -56,10 +56,10 @@ class State_JPLQuatLocal : public ceres::LocalParameterization { bool PlusJacobian(const double *x, double *jacobian) const override; // Inverse update: delta = Log(q2 ⊗ inv(q1)) - bool Minus(const double* y, const double* x, double* delta) const override; + bool Minus(const double *y, const double *x, double *delta) const override; // Jacobian of Minus - bool MinusJacobian(const double* x, double* jacobian) const override; + bool MinusJacobian(const double *x, double *jacobian) const override; int AmbientSize() const override { return 4; } int TangentSize() const override { return 3; } @@ -84,7 +84,6 @@ class State_JPLQuatLocal : public ceres::LocalParameterization { int LocalSize() const override { return 3; }; #endif - }; } // namespace ov_init diff --git a/ov_init/src/dynamic/DynamicInitializer.cpp b/ov_init/src/dynamic/DynamicInitializer.cpp index dc48a11c9..f9d4cadb0 100644 --- a/ov_init/src/dynamic/DynamicInitializer.cpp +++ b/ov_init/src/dynamic/DynamicInitializer.cpp @@ -371,8 +371,8 @@ bool DynamicInitializer::initialize(double ×tamp, Eigen::MatrixXd &covarian } else { H_i.block(0, size_feature * A_index_features.at(feat.first), 2, 3) = Y; // feat } - H_i.block(0, size_feature * num_features + 0, 2, 3) = -DT * Y; // vel - H_i.block(0, size_feature * num_features + 3, 2, 3) = 0.5 * DT * DT * Y; // grav + H_i.block(0, size_feature * num_features + 0, 2, 3) = -DT * Y; // vel + H_i.block(0, size_feature * num_features + 3, 2, 3) = 0.5 * DT * DT * Y; // grav // Else lets append this to our system! A.block(index_meas, 0, 2, A.cols()) = H_i; diff --git a/ov_msckf/launch/display_ros2.rviz b/ov_msckf/launch/display_ros2.rviz index a5f72a058..7ba3d5574 100644 --- a/ov_msckf/launch/display_ros2.rviz +++ b/ov_msckf/launch/display_ros2.rviz @@ -3,14 +3,16 @@ Panels: Help Height: 0 Name: Displays Property Tree Widget: - Expanded: ~ + Expanded: + - /Pose1/Topic1 + - /Image1 + - /Image2 Splitter Ratio: 0.4882352948188782 - Tree Height: 272 + Tree Height: 177 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties Expanded: - - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 @@ -45,13 +47,9 @@ Visualization Manager: Frame Timeout: 120 Frames: All Enabled: true - cam0: - Value: true global: Value: true - imu: - Value: true - truth: + gt_frame: Value: true Marker Scale: 1 Name: TF @@ -60,10 +58,7 @@ Visualization Manager: Show Names: true Tree: global: - imu: - cam0: - {} - truth: + gt_frame: {} Update Interval: 0 Value: true @@ -74,9 +69,12 @@ Visualization Manager: Min Value: 0 Name: Image Tracks Normalize Range: true - Queue Size: 10 - Topic: /ov_msckf/trackhist - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /ov_msckf/trackhist Value: true - Class: rviz_default_plugins/Image Enabled: false @@ -85,9 +83,12 @@ Visualization Manager: Min Value: 0 Name: Current Depths Normalize Range: true - Queue Size: 10 - Topic: /ov_msckf/loop_depth_colored - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /ov_msckf/loop_depth_colored Value: false - Alpha: 1 Buffer Length: 1 @@ -109,8 +110,13 @@ Visualization Manager: Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 - Topic: /ov_msckf/pathimu - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /ov_msckf/pathimu Value: true - Alpha: 1 Buffer Length: 1 @@ -132,8 +138,13 @@ Visualization Manager: Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 - Topic: /ov_msckf/pathgt - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /gt_path Value: true - Alpha: 0.800000011920929 Autocompute Intensity Bounds: true @@ -155,13 +166,17 @@ Visualization Manager: Min Intensity: 0 Name: MSCKF Points Position Transformer: XYZ - Queue Size: 10 Selectable: true Size (Pixels): 8 Size (m): 0.009999999776482582 Style: Points - Topic: /ov_msckf/points_msckf - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /ov_msckf/points_msckf Use Fixed Frame: true Use rainbow: true Value: true @@ -185,13 +200,17 @@ Visualization Manager: Min Intensity: 0 Name: SLAM Points Position Transformer: XYZ - Queue Size: 10 Selectable: true Size (Pixels): 8 Size (m): 0.009999999776482582 Style: Points - Topic: /ov_msckf/points_slam - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /ov_msckf/points_slam Use Fixed Frame: true Use rainbow: true Value: true @@ -215,13 +234,17 @@ Visualization Manager: Min Intensity: 0 Name: ARUCO Points Position Transformer: XYZ - Queue Size: 10 Selectable: true Size (Pixels): 15 Size (m): 0.009999999776482582 Style: Points - Topic: /ov_msckf/points_aruco - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /ov_msckf/points_aruco Use Fixed Frame: true Use rainbow: true Value: true @@ -245,13 +268,17 @@ Visualization Manager: Min Intensity: 0 Name: ACTIVE Points Position Transformer: XYZ - Queue Size: 10 Selectable: true Size (Pixels): 3 Size (m): 0.009999999776482582 Style: Points - Topic: /ov_msckf/loop_feats - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /ov_msckf/loop_feats Use Fixed Frame: true Use rainbow: true Value: true @@ -275,16 +302,68 @@ Visualization Manager: Min Intensity: 0 Name: SIM Points Position Transformer: XYZ - Queue Size: 10 Selectable: true Size (Pixels): 3 Size (m): 0.009999999776482582 Style: Points - Topic: /ov_msckf/points_sim - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /ov_msckf/points_sim Use Fixed Frame: true Use rainbow: true Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Pose + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: + Depth: 1000 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep All + Reliability Policy: Reliable + Value: /odometry + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cam0/masked + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cam0/segmentation + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -298,12 +377,30 @@ Visualization Manager: - Class: rviz_default_plugins/Measure Line color: 128; 128; 0 - Class: rviz_default_plugins/SetInitialPose - Topic: /initialpose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose - Class: rviz_default_plugins/SetGoal - Topic: /move_base_simple/goal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /move_base_simple/goal - Class: rviz_default_plugins/PublishPoint Single click: true - Topic: /clicked_point + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point Transformation: Current: Class: rviz_default_plugins/TF @@ -311,7 +408,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 10 + Distance: 32.85626220703125 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -336,18 +433,20 @@ Window Geometry: collapsed: false Displays: collapsed: false - Height: 690 + Height: 730 Hide Left Dock: false Hide Right Dock: true + Image: + collapsed: false Image Tracks: collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a00000258fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000014d000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000180049006d00610067006500200054007200610063006b00730100000190000001050000002800fffffffb0000000a0049006d00610067006501000001c60000011c0000000000000000fb0000001c00430075007200720065006e0074002000440065007000740068007300000001eb000000b50000002800ffffff000000010000010f00000218fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000218000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000039b0000025800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000016a00000280fc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000000ee000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000180049006d00610067006500200054007200610063006b007301000001310000006a0000002800fffffffb0000000a0049006d00610067006501000001a10000008c0000002800fffffffb0000001c00430075007200720065006e00740020004400650070007400680073000000020c0000007d0000002800fffffffb0000000a0049006d00610067006501000002330000008a0000002800fffffffb0000000a0049006d00610067006501000001f4000000a10000000000000000000000010000010f00000218fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000218000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000003470000028000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Tool Properties: collapsed: false Views: collapsed: true - Width: 1291 - X: 2051 - Y: 110 + Width: 1207 + X: 272 + Y: 92 diff --git a/ov_msckf/src/core/VioManagerOptions.h b/ov_msckf/src/core/VioManagerOptions.h index 0df7d159c..58ce69b8a 100644 --- a/ov_msckf/src/core/VioManagerOptions.h +++ b/ov_msckf/src/core/VioManagerOptions.h @@ -219,6 +219,15 @@ struct VioManagerOptions { /// Mask images for each camera std::map masks; + /// If we should use dynamic masks from ROS topics + bool use_dynamic_mask = false; + + /// ROS topic for left camera mask + std::string dynamic_mask_topic0 = "/mask0"; + + /// ROS topic for right camera mask + std::string dynamic_mask_topic1 = "/mask1"; + /** * @brief This function will load and print all state parameters (e.g. sensor extrinsics) * This allows for visual checking that everything was loaded properly from ROS/CMD parsers. @@ -279,6 +288,16 @@ struct VioManagerOptions { camera_extrinsics.insert({i, cam_eigen}); } parser->parse_config("use_mask", use_mask); + parser->parse_config("use_dynamic_mask", use_dynamic_mask); + parser->parse_config("dynamic_mask_topic0", dynamic_mask_topic0); + parser->parse_config("dynamic_mask_topic1", dynamic_mask_topic1); + + PRINT_DEBUG(" - dynamic masks?: %d\n", use_dynamic_mask); + if (use_dynamic_mask) { + PRINT_DEBUG(" - dynamic mask topic 0: %s\n", dynamic_mask_topic0.c_str()); + PRINT_DEBUG(" - dynamic mask topic 1: %s\n", dynamic_mask_topic1.c_str()); + } + if (use_mask) { for (int i = 0; i < state_options.num_cameras; i++) { std::string mask_path; diff --git a/ov_msckf/src/ros/ROS1Visualizer.cpp b/ov_msckf/src/ros/ROS1Visualizer.cpp index 6e1cf127d..28870a41c 100644 --- a/ov_msckf/src/ros/ROS1Visualizer.cpp +++ b/ov_msckf/src/ros/ROS1Visualizer.cpp @@ -169,27 +169,65 @@ void ROS1Visualizer::setup_subscribers(std::shared_ptr pars _nh->param("topic_camera" + std::to_string(1), cam_topic1, "/cam" + std::to_string(1) + "/image_raw"); parser->parse_external("relative_config_imucam", "cam" + std::to_string(0), "rostopic", cam_topic0); parser->parse_external("relative_config_imucam", "cam" + std::to_string(1), "rostopic", cam_topic1); - // Create sync filter (they have unique pointers internally, so we have to use move logic here...) + + // Create image subscribers auto image_sub0 = std::make_shared>(*_nh, cam_topic0, 1); auto image_sub1 = std::make_shared>(*_nh, cam_topic1, 1); - auto sync = std::make_shared>(sync_pol(10), *image_sub0, *image_sub1); - sync->registerCallback(boost::bind(&ROS1Visualizer::callback_stereo, this, _1, _2, 0, 1)); - // Append to our vector of subscribers - sync_cam.push_back(sync); - sync_subs_cam.push_back(image_sub0); - sync_subs_cam.push_back(image_sub1); - PRINT_INFO("subscribing to cam (stereo): %s\n", cam_topic0.c_str()); - PRINT_INFO("subscribing to cam (stereo): %s\n", cam_topic1.c_str()); + + // NEW LOGIC FOR DYNAMIC MASKS + if (_app->get_params().use_dynamic_mask) { + std::string mask_topic0 = _app->get_params().dynamic_mask_topic0; + std::string mask_topic1 = _app->get_params().dynamic_mask_topic1; + PRINT_INFO("subscribing to stereo masks: - %s - %s", mask_topic0.c_str(), mask_topic1.c_str()); + + auto mask_sub0 = std::make_shared>(*_nh, mask_topic0, 1); + auto mask_sub1 = std::make_shared>(*_nh, mask_topic1, 1); + + auto sync = std::make_shared>(sync_pol_masks(10), *image_sub0, *image_sub1, *mask_sub0, + *mask_sub1); + sync->registerCallback(boost::bind(&ROS1Visualizer::callback_stereo_masks, this, _1, _2, _3, _4, 0, 1)); + + sync_cam_masks.push_back(sync); + sync_subs_cam.push_back(image_sub0); + sync_subs_cam.push_back(image_sub1); + sync_subs_cam.push_back(mask_sub0); + sync_subs_cam.push_back(mask_sub1); + } else { + // ORIGINAL LOGIC + auto sync = std::make_shared>(sync_pol(10), *image_sub0, *image_sub1); + sync->registerCallback(boost::bind(&ROS1Visualizer::callback_stereo, this, _1, _2, 0, 1)); + sync_cam.push_back(sync); + sync_subs_cam.push_back(image_sub0); + sync_subs_cam.push_back(image_sub1); + } + PRINT_INFO("subscribing to cam (stereo): %s", cam_topic0.c_str()); + PRINT_INFO("subscribing to cam (stereo): %s", cam_topic1.c_str()); } else { // Now we should add any non-stereo callbacks here for (int i = 0; i < _app->get_params().state_options.num_cameras; i++) { - // read in the topic + // Read in the camera topic std::string cam_topic; - _nh->param("topic_camera" + std::to_string(i), cam_topic, "/cam" + std::to_string(i) + "/image_raw"); + _node->declare_parameter("topic_camera" + std::to_string(i), "/cam" + std::to_string(i) + "/image_raw"); + _node->get_parameter("topic_camera" + std::to_string(i), cam_topic); parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "rostopic", cam_topic); - // create subscriber - subs_cam.push_back(_nh->subscribe(cam_topic, 10, boost::bind(&ROS1Visualizer::callback_monocular, this, _1, i))); - PRINT_INFO("subscribing to cam (mono): %s\n", cam_topic.c_str()); + + if (_app->get_params().use_dynamic_mask) { + std::string mask_topic = (i == 0) ? _app->get_params().dynamic_mask_topic0 : _app->get_params().dynamic_mask_topic1; + auto image_sub = std::make_shared>(*_nh, cam_topic, 1); + auto mask_sub = std::make_shared>(*_nh, mask_topic, 1); + auto sync = std::make_shared>(sync_pol_mono_mask(10), *image_sub, *mask_sub); + sync->registerCallback(boost::bind(&ROS1Visualizer::callback_monocular_masks, this, _1, _2, i)); + sync_cam_mono_masks.push_back(sync); + sync_subs_cam.push_back(image_sub); + sync_subs_cam.push_back(mask_sub); + } else { + // Original monocular logic + auto sub = _node->create_subscription( + cam_topic, 10, [this, i](const sensor_msgs::msg::Image::SharedPtr msg0) { callback_monocular(msg0, i); }); + subs_cam.push_back(sub); + } + PRINT_INFO("subscribing to cam (mono): %s +", cam_topic.c_str()); } } } @@ -588,6 +626,137 @@ void ROS1Visualizer::callback_stereo(const sensor_msgs::ImageConstPtr &msg0, con std::sort(camera_queue.begin(), camera_queue.end()); } +void ROS1Visualizer::callback_stereo_masks(const sensor_msgs::ImageConstPtr &msg0, const sensor_msgs::ImageConstPtr &msg1, + const sensor_msgs::ImageConstPtr &mask0, const sensor_msgs::ImageConstPtr &mask1, int cam_id0, + int cam_id1) { + + // Check if we should drop this image (throttling) + double timestamp = msg0->header.stamp.toSec(); + double time_delta = 1.0 / _app->get_params().track_frequency; + if (camera_last_timestamp.find(cam_id0) != camera_last_timestamp.end() && timestamp < camera_last_timestamp.at(cam_id0) + time_delta) { + return; + } + camera_last_timestamp[cam_id0] = timestamp; + + // Get the images and masks + cv_bridge::CvImageConstPtr cv_ptr0, cv_ptr1, cv_mask0, cv_mask1; + try { + cv_ptr0 = cv_bridge::toCvShare(msg0, sensor_msgs::image_encodings::MONO8); + cv_ptr1 = cv_bridge::toCvShare(msg1, sensor_msgs::image_encodings::MONO8); + cv_mask0 = cv_bridge::toCvShare(mask0, sensor_msgs::image_encodings::MONO8); + cv_mask1 = cv_bridge::toCvShare(mask1, sensor_msgs::image_encodings::MONO8); + } catch (cv_bridge::Exception &e) { + PRINT_ERROR("cv_bridge exception: %s +", e.what()); + return; + } + + // Create the measurement + ov_core::CameraData message; + message.timestamp = cv_ptr0->header.stamp.toSec(); + message.sensor_ids.push_back(cam_id0); + message.sensor_ids.push_back(cam_id1); + message.images.push_back(cv_ptr0->image.clone()); + message.images.push_back(cv_ptr1->image.clone()); + + // === MASK COMBINATION LOGIC === + // 1. Get the dynamic masks from ROS message + cv::Mat dyn_mask0 = cv_mask0->image; + cv::Mat dyn_mask1 = cv_mask1->image; + + // 2. Get the static masks if enabled (from config) + cv::Mat final_mask0, final_mask1; + + if (_app->get_params().use_mask) { + // Merge Static + Dynamic for Cam 0 + cv::Mat static_mask0 = _app->get_params().masks.at(cam_id0); + if (!dyn_mask0.empty() && !static_mask0.empty()) { + cv::bitwise_or(dyn_mask0, static_mask0, final_mask0); + } else { + final_mask0 = dyn_mask0.empty() ? static_mask0 : dyn_mask0; + } + + // Merge Static + Dynamic for Cam 1 + cv::Mat static_mask1 = _app->get_params().masks.at(cam_id1); + if (!dyn_mask1.empty() && !static_mask1.empty()) { + cv::bitwise_or(dyn_mask1, static_mask1, final_mask1); + } else { + final_mask1 = dyn_mask1.empty() ? static_mask1 : dyn_mask1; + } + } else { + // Only use dynamic masks + final_mask0 = dyn_mask0.clone(); + final_mask1 = dyn_mask1.clone(); + } + + // 3. Ensure we have valid masks (black if empty) + if (final_mask0.empty()) + final_mask0 = cv::Mat::zeros(cv_ptr0->image.rows, cv_ptr0->image.cols, CV_8UC1); + if (final_mask1.empty()) + final_mask1 = cv::Mat::zeros(cv_ptr1->image.rows, cv_ptr1->image.cols, CV_8UC1); + + message.masks.push_back(final_mask0); + message.masks.push_back(final_mask1); + // ============================== + + // Append to our queue of images + std::lock_guard lck(camera_queue_mtx); + camera_queue.push_back(message); + std::sort(camera_queue.begin(), camera_queue.end()); +} + +void ROS1Visualizer::callback_monocular_masks(const sensor_msgs::msg::Image::ConstSharedPtr msg, + const sensor_msgs::msg::Image::ConstSharedPtr mask, int cam_id) { + // Throttling logic + double timestamp = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9; + double time_delta = 1.0 / _app->get_params().track_frequency; + if (camera_last_timestamp.find(cam_id) != camera_last_timestamp.end() && timestamp < camera_last_timestamp.at(cam_id) + time_delta) { + return; + } + camera_last_timestamp[cam_id] = timestamp; + + // Get image and mask + cv_bridge::CvImageConstPtr cv_ptr, cv_mask; + try { + cv_ptr = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::MONO8); + cv_mask = cv_bridge::toCvShare(mask, sensor_msgs::image_encodings::MONO8); + } catch (cv_bridge::Exception &e) { + PRINT_ERROR("cv_bridge exception: %s", e.what()); + return; + } + + // Create measurement + ov_core::CameraData message; + message.timestamp = timestamp; + message.sensor_ids.push_back(cam_id); + message.images.push_back(cv_ptr->image.clone()); + + // Merge static and dynamic masks + cv::Mat final_mask; + cv::Mat dyn_mask = cv_mask->image; + if (_app->get_params().use_mask) { + cv::Mat static_mask = _app->get_params().masks.at(cam_id); + if (!dyn_mask.empty() && !static_mask.empty()) { + if (dyn_mask.rows != static_mask.rows || dyn_mask.cols != static_mask.cols) { + PRINT_WARNING(YELLOW "Dynamic mask size mismatch for Cam! Resizing...\n" RESET); + cv::resize(dyn_mask, dyn_mask, static_mask.size(), 0, 0, cv::INTER_NEAREST); + } + cv::bitwise_or(dyn_mask, static_mask, final_mask); + } else { + final_mask = dyn_mask.empty() ? static_mask : dyn_mask; + } + } else { + final_mask = dyn_mask.clone(); + } + if (final_mask.empty()) + final_mask = cv::Mat::zeros(cv_ptr->image.rows, cv_ptr->image.cols, CV_8UC1); + message.masks.push_back(final_mask); + + std::lock_guard lck(camera_queue_mtx); + camera_queue.push_back(message); + std::sort(camera_queue.begin(), camera_queue.end()); +} + void ROS1Visualizer::publish_state() { // Get the current state diff --git a/ov_msckf/src/ros/ROS1Visualizer.h b/ov_msckf/src/ros/ROS1Visualizer.h index 9764a7bcc..a09cbd0e1 100644 --- a/ov_msckf/src/ros/ROS1Visualizer.h +++ b/ov_msckf/src/ros/ROS1Visualizer.h @@ -114,6 +114,10 @@ class ROS1Visualizer { /// Callback for synchronized stereo camera information void callback_stereo(const sensor_msgs::ImageConstPtr &msg0, const sensor_msgs::ImageConstPtr &msg1, int cam_id0, int cam_id1); + /// Callback for synchronized stereo camera information WITH dynamic masks + void callback_stereo_masks(const sensor_msgs::ImageConstPtr &msg0, const sensor_msgs::ImageConstPtr &msg1, + const sensor_msgs::ImageConstPtr &mask0, const sensor_msgs::ImageConstPtr &mask1, int cam_id0, int cam_id1); + protected: /// Publish the current state void publish_state(); @@ -150,8 +154,15 @@ class ROS1Visualizer { ros::Subscriber sub_imu; std::vector subs_cam; typedef message_filters::sync_policies::ApproximateTime sync_pol; + typedef message_filters::sync_policies::ApproximateTime + sync_pol_masks; + typedef message_filters::sync_policies::ApproximateTime sync_pol_mono_mask; std::vector>> sync_cam; + std::vector>> sync_cam_masks; + std::vector>> sync_cam_mono_masks; std::vector>> sync_subs_cam; + void callback_monocular_masks(const sensor_msgs::msg::Image::ConstSharedPtr msg, const sensor_msgs::msg::Image::ConstSharedPtr mask, + int cam_id); // For path viz unsigned int poses_seq_imu = 0; diff --git a/ov_msckf/src/ros/ROS2Visualizer.cpp b/ov_msckf/src/ros/ROS2Visualizer.cpp index e91cd7430..3ac6f465d 100644 --- a/ov_msckf/src/ros/ROS2Visualizer.cpp +++ b/ov_msckf/src/ros/ROS2Visualizer.cpp @@ -185,35 +185,71 @@ void ROS2Visualizer::setup_subscribers(std::shared_ptr pars _node->get_parameter("topic_camera" + std::to_string(1), cam_topic1); parser->parse_external("relative_config_imucam", "cam" + std::to_string(0), "rostopic", cam_topic0); parser->parse_external("relative_config_imucam", "cam" + std::to_string(1), "rostopic", cam_topic1); + // Create sync filter (they have unique pointers internally, so we have to use move logic here...) auto image_sub0 = std::make_shared>(_node, cam_topic0); auto image_sub1 = std::make_shared>(_node, cam_topic1); - auto sync = std::make_shared>(sync_pol(10), *image_sub0, *image_sub1); - sync->registerCallback(std::bind(&ROS2Visualizer::callback_stereo, this, std::placeholders::_1, std::placeholders::_2, 0, 1)); - // sync->registerCallback([](const sensor_msgs::msg::Image::SharedPtr msg0, const sensor_msgs::msg::Image::SharedPtr msg1) - // {callback_stereo(msg0, msg1, 0, 1);}); - // sync->registerCallback(&callback_stereo2); // since the above two alternatives fail to compile for some reason - // Append to our vector of subscribers - sync_cam.push_back(sync); - sync_subs_cam.push_back(image_sub0); - sync_subs_cam.push_back(image_sub1); - PRINT_INFO("subscribing to cam (stereo): %s\n", cam_topic0.c_str()); - PRINT_INFO("subscribing to cam (stereo): %s\n", cam_topic1.c_str()); + + // NEW LOGIC FOR DYNAMIC MASKS + if (_app->get_params().use_dynamic_mask) { + std::string mask_topic0 = _app->get_params().dynamic_mask_topic0; + std::string mask_topic1 = _app->get_params().dynamic_mask_topic1; + PRINT_INFO("subscribing to stereo masks: - %s - %s", mask_topic0.c_str(), mask_topic1.c_str()); + + auto mask_sub0 = std::make_shared>(_node, mask_topic0); + auto mask_sub1 = std::make_shared>(_node, mask_topic1); + + auto sync = std::make_shared>(sync_pol_masks(10), *image_sub0, *image_sub1, *mask_sub0, + *mask_sub1); + sync->registerCallback(std::bind(&ROS2Visualizer::callback_stereo_masks, this, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, 0, 1)); + + sync_cam_masks.push_back(sync); + sync_subs_cam.push_back(image_sub0); + sync_subs_cam.push_back(image_sub1); + sync_subs_cam.push_back(mask_sub0); // Keep mask subscribers alive + sync_subs_cam.push_back(mask_sub1); + + } else { + // ORIGINAL LOGIC (No dynamic mask) + auto sync = std::make_shared>(sync_pol(10), *image_sub0, *image_sub1); + sync->registerCallback(std::bind(&ROS2Visualizer::callback_stereo, this, std::placeholders::_1, std::placeholders::_2, 0, 1)); + sync_cam.push_back(sync); + sync_subs_cam.push_back(image_sub0); + sync_subs_cam.push_back(image_sub1); + } + PRINT_INFO("subscribing to cam (stereo): %s", cam_topic0.c_str()); + PRINT_INFO("subscribing to cam (stereo): %s", cam_topic1.c_str()); } else { // Now we should add any non-stereo callbacks here for (int i = 0; i < _app->get_params().state_options.num_cameras; i++) { - // read in the topic + // Read in the camera topic std::string cam_topic; _node->declare_parameter("topic_camera" + std::to_string(i), "/cam" + std::to_string(i) + "/image_raw"); _node->get_parameter("topic_camera" + std::to_string(i), cam_topic); parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "rostopic", cam_topic); - // create subscriber - // auto sub = _node->create_subscription( - // cam_topic, rclcpp::SensorDataQoS(), std::bind(&ROS2Visualizer::callback_monocular, this, std::placeholders::_1, i)); - auto sub = _node->create_subscription( - cam_topic, 10, [this, i](const sensor_msgs::msg::Image::SharedPtr msg0) { callback_monocular(msg0, i); }); - subs_cam.push_back(sub); - PRINT_INFO("subscribing to cam (mono): %s\n", cam_topic.c_str()); + + if (_app->get_params().use_dynamic_mask) { + // Use dynamic_mask_topic0 for cam0, dynamic_mask_topic1 for cam1 + std::string mask_topic = (i == 0) ? _app->get_params().dynamic_mask_topic0 : _app->get_params().dynamic_mask_topic1; + PRINT_INFO("subscribing to mono mask for cam%d: %s", i, mask_topic.c_str()); + + auto image_sub = std::make_shared>(_node, cam_topic); + auto mask_sub = std::make_shared>(_node, mask_topic); + + auto sync = std::make_shared>(sync_pol_mono_mask(10), *image_sub, *mask_sub); + sync->registerCallback(std::bind(&ROS2Visualizer::callback_monocular_masks, this, std::placeholders::_1, std::placeholders::_2, i)); + + sync_cam_mono_masks.push_back(sync); + sync_subs_cam.push_back(image_sub); + sync_subs_cam.push_back(mask_sub); + } else { + // Original monocular logic + auto sub = _node->create_subscription( + cam_topic, 10, [this, i](const sensor_msgs::msg::Image::SharedPtr msg0) { callback_monocular(msg0, i); }); + subs_cam.push_back(sub); + } + PRINT_INFO("subscribing to cam (mono): %s", cam_topic.c_str()); } } } @@ -588,6 +624,147 @@ void ROS2Visualizer::callback_stereo(const sensor_msgs::msg::Image::ConstSharedP std::sort(camera_queue.begin(), camera_queue.end()); } +void ROS2Visualizer::callback_stereo_masks(const sensor_msgs::msg::Image::ConstSharedPtr msg0, + const sensor_msgs::msg::Image::ConstSharedPtr msg1, + const sensor_msgs::msg::Image::ConstSharedPtr mask0, + const sensor_msgs::msg::Image::ConstSharedPtr mask1, int cam_id0, int cam_id1) { + + // Check if we should drop this image (throttling) + double timestamp = msg0->header.stamp.sec + msg0->header.stamp.nanosec * 1e-9; + double time_delta = 1.0 / _app->get_params().track_frequency; + if (camera_last_timestamp.find(cam_id0) != camera_last_timestamp.end() && timestamp < camera_last_timestamp.at(cam_id0) + time_delta) { + return; + } + camera_last_timestamp[cam_id0] = timestamp; + + // Get the images + cv_bridge::CvImageConstPtr cv_ptr0, cv_ptr1, cv_mask0, cv_mask1; + try { + cv_ptr0 = cv_bridge::toCvShare(msg0, sensor_msgs::image_encodings::MONO8); + cv_ptr1 = cv_bridge::toCvShare(msg1, sensor_msgs::image_encodings::MONO8); + // Ensure masks are mono8 + cv_mask0 = cv_bridge::toCvShare(mask0, sensor_msgs::image_encodings::MONO8); + cv_mask1 = cv_bridge::toCvShare(mask1, sensor_msgs::image_encodings::MONO8); + } catch (cv_bridge::Exception &e) { + PRINT_ERROR("cv_bridge exception: %s", e.what()); + return; + } + + // Create the measurement + ov_core::CameraData message; + message.timestamp = cv_ptr0->header.stamp.sec + cv_ptr0->header.stamp.nanosec * 1e-9; + message.sensor_ids.push_back(cam_id0); + message.sensor_ids.push_back(cam_id1); + message.images.push_back(cv_ptr0->image.clone()); + message.images.push_back(cv_ptr1->image.clone()); + + // === MASK COMBINATION LOGIC === + // 1. Get the dynamic masks from ROS message + cv::Mat dyn_mask0 = cv_mask0->image; + cv::Mat dyn_mask1 = cv_mask1->image; + + // 2. Get the static masks if enabled (from config) + cv::Mat final_mask0, final_mask1; + + if (_app->get_params().use_mask) { + // Merge Static + Dynamic for Cam 0 + cv::Mat static_mask0 = _app->get_params().masks.at(cam_id0); + if (!dyn_mask0.empty() && !static_mask0.empty()) { + if (dyn_mask0.rows != static_mask0.rows || dyn_mask0.cols != static_mask0.cols) { + PRINT_WARNING(YELLOW "Dynamic mask size mismatch for Cam0! Resizing...\n" RESET); + cv::resize(dyn_mask0, dyn_mask0, static_mask0.size(), 0, 0, cv::INTER_NEAREST); + } + cv::bitwise_or(dyn_mask0, static_mask0, final_mask0); + } else { + final_mask0 = dyn_mask0.empty() ? static_mask0 : dyn_mask0; + } + + // Merge Static + Dynamic for Cam 1 + cv::Mat static_mask1 = _app->get_params().masks.at(cam_id1); + if (!dyn_mask1.empty() && !static_mask1.empty()) { + if (dyn_mask1.rows != static_mask0.rows || dyn_mask1.cols != static_mask0.cols) { + PRINT_WARNING(YELLOW "Dynamic mask size mismatch for Cam1! Resizing...\n" RESET); + cv::resize(dyn_mask1, dyn_mask1, static_mask0.size(), 0, 0, cv::INTER_NEAREST); + } + cv::bitwise_or(dyn_mask1, static_mask1, final_mask1); + } else { + final_mask1 = dyn_mask1.empty() ? static_mask1 : dyn_mask1; + } + + } else { + // Only use dynamic masks + final_mask0 = dyn_mask0.clone(); + final_mask1 = dyn_mask1.clone(); + } + + // 3. Ensure we have valid masks (black if empty) + if (final_mask0.empty()) + final_mask0 = cv::Mat::zeros(cv_ptr0->image.rows, cv_ptr0->image.cols, CV_8UC1); + if (final_mask1.empty()) + final_mask1 = cv::Mat::zeros(cv_ptr1->image.rows, cv_ptr1->image.cols, CV_8UC1); + + message.masks.push_back(final_mask0); + message.masks.push_back(final_mask1); + // ============================== + + // Append to our queue of images + std::lock_guard lck(camera_queue_mtx); + camera_queue.push_back(message); + std::sort(camera_queue.begin(), camera_queue.end()); +} + +void ROS2Visualizer::callback_monocular_masks(const sensor_msgs::msg::Image::ConstSharedPtr msg, + const sensor_msgs::msg::Image::ConstSharedPtr mask, int cam_id) { + // Throttling logic + double timestamp = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9; + double time_delta = 1.0 / _app->get_params().track_frequency; + if (camera_last_timestamp.find(cam_id) != camera_last_timestamp.end() && timestamp < camera_last_timestamp.at(cam_id) + time_delta) { + return; + } + camera_last_timestamp[cam_id] = timestamp; + + // Get image and mask + cv_bridge::CvImageConstPtr cv_ptr, cv_mask; + try { + cv_ptr = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::MONO8); + cv_mask = cv_bridge::toCvShare(mask, sensor_msgs::image_encodings::MONO8); + } catch (cv_bridge::Exception &e) { + PRINT_ERROR("cv_bridge exception: %s", e.what()); + return; + } + + // Create measurement + ov_core::CameraData message; + message.timestamp = timestamp; + message.sensor_ids.push_back(cam_id); + message.images.push_back(cv_ptr->image.clone()); + + // Merge static and dynamic masks + cv::Mat final_mask; + cv::Mat dyn_mask = cv_mask->image; + if (_app->get_params().use_mask) { + cv::Mat static_mask = _app->get_params().masks.at(cam_id); + if (!dyn_mask.empty() && !static_mask.empty()) { + if (dyn_mask.rows != static_mask.rows || dyn_mask.cols != static_mask.cols) { + PRINT_WARNING(YELLOW "Dynamic mask size mismatch for Cam! Resizing...\n" RESET); + cv::resize(dyn_mask, dyn_mask, static_mask.size(), 0, 0, cv::INTER_NEAREST); + } + cv::bitwise_or(dyn_mask, static_mask, final_mask); + } else { + final_mask = dyn_mask.empty() ? static_mask : dyn_mask; + } + } else { + final_mask = dyn_mask.clone(); + } + if (final_mask.empty()) + final_mask = cv::Mat::zeros(cv_ptr->image.rows, cv_ptr->image.cols, CV_8UC1); + message.masks.push_back(final_mask); + + std::lock_guard lck(camera_queue_mtx); + camera_queue.push_back(message); + std::sort(camera_queue.begin(), camera_queue.end()); +} + void ROS2Visualizer::publish_state() { // Get the current state diff --git a/ov_msckf/src/ros/ROS2Visualizer.h b/ov_msckf/src/ros/ROS2Visualizer.h index a737f80aa..96197a783 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 @@ -119,6 +119,11 @@ class ROS2Visualizer { void callback_stereo(const sensor_msgs::msg::Image::ConstSharedPtr msg0, const sensor_msgs::msg::Image::ConstSharedPtr msg1, int cam_id0, int cam_id1); + /// Callback for synchronized stereo camera information WITH dynamic masks + void callback_stereo_masks(const sensor_msgs::msg::Image::ConstSharedPtr msg0, const sensor_msgs::msg::Image::ConstSharedPtr msg1, + const sensor_msgs::msg::Image::ConstSharedPtr mask0, const sensor_msgs::msg::Image::ConstSharedPtr mask1, + int cam_id0, int cam_id1); + protected: /// Publish the current state void publish_state(); @@ -159,8 +164,16 @@ class ROS2Visualizer { rclcpp::Subscription::SharedPtr sub_imu; std::vector::SharedPtr> subs_cam; typedef message_filters::sync_policies::ApproximateTime sync_pol; + typedef message_filters::sync_policies::ApproximateTime + sync_pol_masks; + typedef message_filters::sync_policies::ApproximateTime sync_pol_mono_mask; std::vector>> sync_cam; std::vector>> sync_subs_cam; + std::vector>> sync_cam_masks; + std::vector>> sync_cam_mono_masks; + void callback_monocular_masks(const sensor_msgs::msg::Image::ConstSharedPtr msg, const sensor_msgs::msg::Image::ConstSharedPtr mask, + int cam_id); // For path viz std::vector poses_imu; diff --git a/ov_msckf/src/ros/ROSVisualizerHelper.h b/ov_msckf/src/ros/ROSVisualizerHelper.h index 14c1514c6..4dff3043e 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 {