add model

CMakeLists.txt

@@ -91,6 +91,7 @@ find_package(catkin REQUIRED COMPONENTS
   vikit_ros
   cv_bridge
   image_transport
+  message_filters
 )
 
 find_package(Eigen3 REQUIRED)
@@ -103,7 +104,7 @@ set(Sophus_LIBRARIES libSophus.so)
 
 # Define the catkin package
 catkin_package(
-  CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs message_runtime cv_bridge vikit_common vikit_ros image_transport
+  CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs message_runtime cv_bridge vikit_common vikit_ros image_transport message_filters
   DEPENDS EIGEN3 PCL OpenCV Sophus
 )
 

config/camera_hk_mems.yaml

@@ -0,0 +1,12 @@
+cam_model: Pinhole
+cam_width: 1280
+cam_height: 720
+scale: 1
+cam_fx: 897.4596557617188
+cam_fy: 657.0736083984375
+cam_cx: 897.8496704101562
+cam_cy: 358.958251953125
+cam_d0: 0.1318076252937317
+cam_d1: -0.449052631855011
+cam_d2: 0.0010584293631836772
+cam_d3: 0.0007639264222234488

config/camera_urban_loco_ca.yaml

@@ -0,0 +1,12 @@
+cam_model: Pinhole
+cam_width: 2048
+cam_height: 1536
+scale: 1
+cam_fx: 1857.4752797615988
+cam_fy: 1869.2155909761746
+cam_cx: 1039.692658811044
+cam_cy: 739.3362262994145
+cam_d0:  -3.3474604323919716e-01  # k1
+cam_d1:  2.0887608302549923e-01 # k2
+cam_d2:  8.4845632526369460e-04   # p1
+cam_d3:  4.2905149321811908e-04  # p2

config/camera_urban_loco_hk.yaml

@@ -0,0 +1,12 @@
+cam_model: Pinhole
+cam_width: 2448
+cam_height: 2048
+scale: 1
+cam_fx: 1086.160899
+cam_fy: 1090.242963
+cam_cx: 940.067502
+cam_cy: 586.740077
+cam_d0: -0.109203
+cam_d1:  0.063536
+cam_d2: -0.003427
+cam_d3: -0.000629

config/hk_mems.yaml

@@ -0,0 +1,93 @@
+common:
+  img_topic: "/camera/color/image_raw/compressed"
+  lid_topic: ""
+  imu_topic: "/imu/data"
+  is_compressed_image: true
+  dynamic_slam: true
+  mask_topic: "/camera/color/segmentation_image"
+  img_en: 1             # Enable visual mode
+  lidar_en: 1           # Enable lidar mode
+  ros_driver_bug_fix: false
+
+extrin_calib:
+  # Hilti-2022
+  extrinsic_T: [0, 0, 0] 
+  extrinsic_R: [ 1, 0, 0, 
+                 0, 1, 0, 
+                 0, 0, 1]
+
+
+  Rcl: [ 0,  0, -1,
+         0, 1, 0,
+         1, 0, 0]
+  Pcl: [0, 0, 0 ]
+
+time_offset:
+  imu_time_offset: 0.0
+  img_time_offset: 0.0
+  exposure_time_init: 0.0
+
+preprocess:
+  point_filter_num: 3
+  filter_size_surf: 0.1
+  lidar_type: 3 # Ouster
+  scan_line: 32
+  blind: 0.5
+
+vio:
+  max_iterations: 5
+  outlier_threshold: 500
+  img_point_cov: 1000
+  patch_size: 8
+  patch_pyrimid_level: 4
+  normal_en: true
+  raycast_en: false
+  inverse_composition_en: false
+  exposure_estimate_en: true
+  inv_expo_cov: 0.1
+
+imu:
+  imu_en: true
+  imu_int_frame: 30
+  acc_cov: 0.5 # 0.1
+  gyr_cov: 0.01
+  b_acc_cov: 0.0001 # 0.1
+  b_gyr_cov: 0.0001 # 0.1
+
+lio:
+  max_iterations: 5
+  dept_err: 0.02
+  beam_err: 0.05
+  min_eigen_value: 0.0001 # 0.0025
+  voxel_size: 0.4
+  max_layer: 2
+  max_points_num: 100
+  layer_init_num: [5, 5, 5, 5, 5]
+
+local_map:  
+  map_sliding_en: false
+  half_map_size: 100
+  sliding_thresh: 8
+
+uav:
+  imu_rate_odom: false
+  gravity_align_en: false
+
+publish:
+  dense_map_en: true
+  pub_effect_point_en: false 
+  pub_plane_en: false
+  pub_scan_num: 1
+  blind_rgb_points: 0.0
+
+evo:
+  seq_name: "exp09_cupola"
+  pose_output_en: true
+
+pcd_save:
+  pcd_save_en: false
+  colmap_output_en: false # need to set interval = -1
+  filter_size_pcd: 0.15
+  interval: -1
+  # how many LiDAR frames saved in each pcd file;
+  # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.

config/urban_loco_ca.yaml

@@ -0,0 +1,90 @@
+common:
+  img_topic: "/cam0/image_raw"
+  lid_topic: "/rslidar_points_ring"
+  imu_topic: "/imu_raw"
+  img_en: 1             # Enable visual mode
+  lidar_en: 1           # Enable lidar mode
+  ros_driver_bug_fix: false
+
+extrin_calib:
+  # Hilti-2022
+  extrinsic_T: [0, 0, 0] 
+  extrinsic_R: [ 1, 0, 0, 
+                0,  1, 0, 
+                 0,  0, 1]
+
+
+  Rcl: [0, 0, 1,
+        -1, 0, 0,
+        0, -1, 0]
+  Pcl: [  1.8499563388803192e-01, -2.4227446820327757e-02, -3.3589470159409335e-01]
+
+time_offset:
+  imu_time_offset: 0.0
+  img_time_offset: 0.0
+  exposure_time_init: 0.0
+
+preprocess:
+  point_filter_num: 3
+  filter_size_surf: 0.1
+  lidar_type: 8 # Urban Loco 
+  scan_line: 32
+  blind: 0.5
+
+vio:
+  max_iterations: 5
+  outlier_threshold: 500
+  img_point_cov: 1000
+  patch_size: 8
+  patch_pyrimid_level: 4
+  normal_en: true
+  raycast_en: false
+  inverse_composition_en: false
+  exposure_estimate_en: true
+  inv_expo_cov: 0.1
+
+imu:
+  imu_en: true
+  imu_int_frame: 30
+  acc_cov: 0.5 # 0.1
+  gyr_cov: 0.01
+  b_acc_cov: 0.0001 # 0.1
+  b_gyr_cov: 0.0001 # 0.1
+
+lio:
+  max_iterations: 5
+  dept_err: 0.02
+  beam_err: 0.05
+  min_eigen_value: 0.0001 # 0.0025
+  voxel_size: 0.4
+  max_layer: 2
+  max_points_num: 100
+  layer_init_num: [5, 5, 5, 5, 5]
+
+local_map:  
+  map_sliding_en: false
+  half_map_size: 100
+  sliding_thresh: 8
+
+uav:
+  imu_rate_odom: false
+  gravity_align_en: false
+
+publish:
+  dense_map_en: true
+  pub_effect_point_en: false 
+  pub_plane_en: false
+  pub_scan_num: 1
+  blind_rgb_points: 0.0
+
+evo:
+  seq_name: "exp09_cupola"
+  pose_output_en: true
+
+pcd_save:
+  pcd_save_en: false
+  colmap_output_en: false # need to set interval = -1
+  filter_size_pcd: 0.15
+  interval: -1
+  # how many LiDAR frames saved in each pcd file;
+  # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.

config/urban_loco_hk.yaml

@@ -0,0 +1,90 @@
+common:
+  img_topic: "/camera/image_color"
+  lid_topic: "/velodyne_points"
+  imu_topic: "/imu/data"
+  img_en: 1             # Enable visual mode
+  lidar_en: 0           # Enable lidar mode
+  ros_driver_bug_fix: false
+
+extrin_calib:
+  # Hilti-2022
+  extrinsic_T: [0, 0, -0.28] 
+  extrinsic_R: [ 2.67949e-08, -1, 0, 
+                 1,  2.67949e-08, 0, 
+                 0,   0, 1]
+
+
+  Rcl: [ 9.9934608718980233e-01, -1.5715484428488590e-02,-3.2564114721728155e-02,
+         3.2359037356803094e-02, -1.3131917124154624e-02,9.9939003669937865e-01,
+         3.2359037356803094e-02, -1.3131917124154624e-02,9.9939003669937865e-01]
+  Pcl: [ -1.7434527332030676e-02, 1.7171139776467173e-01, -4.5251036141047592e-02 ]
+
+time_offset:
+  imu_time_offset: 0.0
+  img_time_offset: 0.0
+  exposure_time_init: 0.0
+
+preprocess:
+  point_filter_num: 3
+  filter_size_surf: 0.1
+  lidar_type: 8 # Livox Avia LiDAR
+  scan_line: 32
+  blind: 0.5
+
+vio:
+  max_iterations: 5
+  outlier_threshold: 500
+  img_point_cov: 1000
+  patch_size: 8
+  patch_pyrimid_level: 4
+  normal_en: true
+  raycast_en: false
+  inverse_composition_en: false
+  exposure_estimate_en: true
+  inv_expo_cov: 0.1
+
+imu:
+  imu_en: true
+  imu_int_frame: 30
+  acc_cov: 0.5 # 0.1
+  gyr_cov: 0.01
+  b_acc_cov: 0.0001 # 0.1
+  b_gyr_cov: 0.0001 # 0.1
+
+lio:
+  max_iterations: 5
+  dept_err: 0.02
+  beam_err: 0.05
+  min_eigen_value: 0.0001 # 0.0025
+  voxel_size: 0.4
+  max_layer: 2
+  max_points_num: 100
+  layer_init_num: [5, 5, 5, 5, 5]
+
+local_map:  
+  map_sliding_en: false
+  half_map_size: 100
+  sliding_thresh: 8
+
+uav:
+  imu_rate_odom: false
+  gravity_align_en: false
+
+publish:
+  dense_map_en: true
+  pub_effect_point_en: false 
+  pub_plane_en: false
+  pub_scan_num: 1
+  blind_rgb_points: 0.0
+
+evo:
+  seq_name: "exp09_cupola"
+  pose_output_en: true
+
+pcd_save:
+  pcd_save_en: false
+  colmap_output_en: false # need to set interval = -1
+  filter_size_pcd: 0.15
+  interval: -1
+  # how many LiDAR frames saved in each pcd file;
+  # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.

include/LIVMapper.h

@@ -20,6 +20,12 @@ which is included as part of this source code package.
 #include <image_transport/image_transport.h>
 #include <nav_msgs/Path.h>
 #include <vikit/camera_loader.h>
+#include <sensor_msgs/CompressedImage.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
+typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> ImageSyncPolicy;
+typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::CompressedImage, sensor_msgs::Image> CompressedSyncPolicy;
 
 class LIVMapper
 {
@@ -49,6 +55,9 @@ class LIVMapper
   void livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg_in);
   void imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in);
   void img_cbk(const sensor_msgs::ImageConstPtr &msg_in);
+  void img_mask_cbk(const sensor_msgs::ImageConstPtr &img_msg, const sensor_msgs::ImageConstPtr &mask_msg);
+  void compressed_mask_cbk(const sensor_msgs::CompressedImageConstPtr &img_msg, const sensor_msgs::ImageConstPtr &mask_msg);
+  void compressed_cbk(const sensor_msgs::CompressedImageConstPtr &msg_in);
   void publish_img_rgb(const image_transport::Publisher &pubImage, VIOManagerPtr vio_manager);
   void publish_frame_world(const ros::Publisher &pubLaserCloudFullRes, VIOManagerPtr vio_manager);
   void publish_visual_sub_map(const ros::Publisher &pubSubVisualMap);
@@ -60,18 +69,27 @@ class LIVMapper
   template <typename T> void set_posestamp(T &out);
   template <typename T> void pointBodyToWorld(const Eigen::Matrix<T, 3, 1> &pi, Eigen::Matrix<T, 3, 1> &po);
   template <typename T> Eigen::Matrix<T, 3, 1> pointBodyToWorld(const Eigen::Matrix<T, 3, 1> &pi);
-  cv::Mat getImageFromMsg(const sensor_msgs::ImageConstPtr &img_msg);
 
+
+  // message_filters::Subscriber<sensor_msgs::Image> img_sub;
+  // message_filters::Subscriber<sensor_msgs::CompressedImage> compressed_sub;
+  // message_filters::Subscriber<sensor_msgs::Image> mask_sub;
+  bool dynamic_slam;
+  std::unique_ptr<message_filters::Synchronizer<ImageSyncPolicy>> img_sync;
+  std::unique_ptr<message_filters::Synchronizer<CompressedSyncPolicy>> com_sync;
+  cv::Mat getImageFromMsg(const sensor_msgs::ImageConstPtr &img_msg);
+  cv::Mat getImageFromCompressedMsg(const sensor_msgs::CompressedImageConstPtr &img_msg);
   std::mutex mtx_buffer, mtx_buffer_imu_prop;
   std::condition_variable sig_buffer;
 
   SLAM_MODE slam_mode_;
   std::unordered_map<VOXEL_LOCATION, VoxelOctoTree *> voxel_map;
 
   string root_dir;
-  string lid_topic, imu_topic, seq_name, img_topic;
+  string lid_topic, imu_topic, seq_name, img_topic, mask_topic;
   V3D extT;
   M3D extR;
+  bool is_compressed_image;
 
   int feats_down_size = 0, max_iterations = 0;
 
@@ -122,6 +140,7 @@ class LIVMapper
   deque<double> lid_header_time_buffer;
   deque<sensor_msgs::Imu::ConstPtr> imu_buffer;
   deque<cv::Mat> img_buffer;
+  deque<cv::Mat> mask_buffer;
   deque<double> img_time_buffer;
   vector<pointWithVar> _pv_list;
   vector<double> extrinT;