diff --git a/CMakeLists.txt b/CMakeLists.txt index 0652aab6..fd6ae07a 100755 --- a/CMakeLists.txt +++ b/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 ) diff --git a/config/camera_hk_mems.yaml b/config/camera_hk_mems.yaml new file mode 100644 index 00000000..b2d0cf8d --- /dev/null +++ b/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 diff --git a/config/camera_urban_loco_ca.yaml b/config/camera_urban_loco_ca.yaml new file mode 100644 index 00000000..0363884b --- /dev/null +++ b/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 diff --git a/config/camera_urban_loco_hk.yaml b/config/camera_urban_loco_hk.yaml new file mode 100644 index 00000000..47795761 --- /dev/null +++ b/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 diff --git a/config/hk_mems.yaml b/config/hk_mems.yaml new file mode 100644 index 00000000..d518ead3 --- /dev/null +++ b/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. diff --git a/config/urban_loco_ca.yaml b/config/urban_loco_ca.yaml new file mode 100644 index 00000000..02bb2299 --- /dev/null +++ b/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. diff --git a/config/urban_loco_hk.yaml b/config/urban_loco_hk.yaml new file mode 100644 index 00000000..6abd9e90 --- /dev/null +++ b/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. diff --git a/include/LIVMapper.h b/include/LIVMapper.h index c8101794..a19530d7 100644 --- a/include/LIVMapper.h +++ b/include/LIVMapper.h @@ -20,6 +20,12 @@ which is included as part of this source code package. #include #include #include +#include +#include +#include +#include +typedef message_filters::sync_policies::ApproximateTime ImageSyncPolicy; +typedef message_filters::sync_policies::ApproximateTime 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,8 +69,16 @@ class LIVMapper template void set_posestamp(T &out); template void pointBodyToWorld(const Eigen::Matrix &pi, Eigen::Matrix &po); template Eigen::Matrix pointBodyToWorld(const Eigen::Matrix &pi); - cv::Mat getImageFromMsg(const sensor_msgs::ImageConstPtr &img_msg); + + // message_filters::Subscriber img_sub; + // message_filters::Subscriber compressed_sub; + // message_filters::Subscriber mask_sub; + bool dynamic_slam; + std::unique_ptr> img_sync; + std::unique_ptr> 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; @@ -69,9 +86,10 @@ class LIVMapper std::unordered_map 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 lid_header_time_buffer; deque imu_buffer; deque img_buffer; + deque mask_buffer; deque img_time_buffer; vector _pv_list; vector extrinT; diff --git a/include/common_lib.h b/include/common_lib.h index 9de8a905..14ce6337 100755 --- a/include/common_lib.h +++ b/include/common_lib.h @@ -43,7 +43,8 @@ enum LID_TYPE L515 = 4, XT32 = 5, PANDAR128 = 6, - ROBOSENSE = 7 + ROBOSENSE = 7, + URBANLOCO = 8 }; enum SLAM_MODE { @@ -65,6 +66,7 @@ struct MeasureGroup double lio_time; deque imu; cv::Mat img; + cv::Mat mask; MeasureGroup() { vio_time = 0.0; diff --git a/include/frame.h b/include/frame.h index 8172dcdc..7b9ddd8e 100644 --- a/include/frame.h +++ b/include/frame.h @@ -34,14 +34,16 @@ class Frame : boost::noncopyable SE3 T_f_w_; //!< Transform (f)rame from (w)orld. SE3 T_f_w_prior_; //!< Transform (f)rame from (w)orld provided by the IMU prior. cv::Mat img_; //!< Image of the frame. + cv::Mat mask_; Features fts_; //!< List of features in the image. Frame(vk::AbstractCamera *cam, const cv::Mat &img); + Frame(vk::AbstractCamera *cam, const cv::Mat &img, const cv::Mat &mask); ~Frame(); /// Initialize new frame and create image pyramid. void initFrame(const cv::Mat &img); - + void initFrame(const cv::Mat &img, const cv::Mat &mask); /// Return number of point observations. inline size_t nObs() const { return fts_.size(); } diff --git a/include/preprocess.h b/include/preprocess.h index bf74f609..794fa64c 100755 --- a/include/preprocess.h +++ b/include/preprocess.h @@ -79,6 +79,21 @@ POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point, (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(float, time, time)(std::uint16_t, ring, ring)) /****************/ +/*** URBAN LOCO ***/ +namespace urbanloco_ros +{ +struct EIGEN_ALIGN16 Point +{ + PCL_ADD_POINT4D; + float intensity; + std::uint16_t ring; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; +} // namespace urbanloco_ros +POINT_CLOUD_REGISTER_POINT_STRUCT(urbanloco_ros::Point, + (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(std::uint32_t, ring, ring)) +/****************/ + /*** Ouster ***/ namespace ouster_ros { @@ -173,6 +188,7 @@ class Preprocess void avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg); void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); void velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); + void urbanloco_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); void xt32_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); void Pandar128_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); void robosense_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); diff --git a/include/vio.h b/include/vio.h index d5600b1d..2ba6b775 100755 --- a/include/vio.h +++ b/include/vio.h @@ -130,7 +130,7 @@ class VIOManager vector append_voxel_points; FramePtr new_frame_; cv::Mat img_cp, img_rgb, img_test; - + cv::Mat mask_cp, mask_p; enum CellType { TYPE_MAP = 1, @@ -143,6 +143,8 @@ class VIOManager void updateStateInverse(cv::Mat img, int level); void updateState(cv::Mat img, int level); void processFrame(cv::Mat &img, vector &pg, const unordered_map &feat_map, double img_time); + void processFrame(cv::Mat &img, cv::Mat &mask, vector &pg, const unordered_map &feat_map, double img_time); + void retrieveFromVisualSparseMap(cv::Mat img, cv::Mat mask, vector &pg, const unordered_map &plane_map); void retrieveFromVisualSparseMap(cv::Mat img, vector &pg, const unordered_map &plane_map); void generateVisualMapPoints(cv::Mat img, vector &pg); void setImuToLidarExtrinsic(const V3D &transl, const M3D &rot); diff --git a/launch/mapping_hk_mems.launch b/launch/mapping_hk_mems.launch new file mode 100755 index 00000000..69a802e2 --- /dev/null +++ b/launch/mapping_hk_mems.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/launch/mapping_urban_loco_ca.launch b/launch/mapping_urban_loco_ca.launch new file mode 100755 index 00000000..e61ec4f3 --- /dev/null +++ b/launch/mapping_urban_loco_ca.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/launch/mapping_urban_loco_hk.launch b/launch/mapping_urban_loco_hk.launch new file mode 100755 index 00000000..c0d835b9 --- /dev/null +++ b/launch/mapping_urban_loco_hk.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/package.xml b/package.xml index c2eedfac..f6df65ef 100755 --- a/package.xml +++ b/package.xml @@ -28,7 +28,9 @@ message_generation vikit_common vikit_ros - cv_bridge + cv_bridge + image_transport + message_filters geometry_msgs nav_msgs @@ -42,9 +44,9 @@ vikit_common vikit_ros cv_bridge - image_transport image_transport - + message_filters + rostest rosbag diff --git a/rviz_cfg/hk_mems.rviz b/rviz_cfg/hk_mems.rviz new file mode 100644 index 00000000..fc49d91d --- /dev/null +++ b/rviz_cfg/hk_mems.rviz @@ -0,0 +1,696 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Status1 + - /Axes1 + - /mapping1 + - /mapping1/currPoints1 + - /mapping1/surround1 + - /mapping1/surround1/Autocompute Value Bounds1 + - /mapping1/PointCloud21 + - /Odometry1 + - /Odometry1/Odometry1 + - /Odometry1/Odometry1/Shape1 + - /Path1 + - /currPoints1/Autocompute Value Bounds1 + - /MarkerArray1/Namespaces1 + - /currPoints2/Autocompute Value Bounds1 + - /Odometry2/Shape1 + - /MarkerArray3 + - /MarkerArray4 + - /MarkerArray5 + - /Image1 + - /Image2 + Splitter Ratio: 0.34272301197052 + Tree Height: 258 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Name: Time + SyncMode: 0 + SyncSource: Image +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 1 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: false + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 160 + Reference Frame: + Value: false + - Alpha: 1 + Class: rviz/Axes + Enabled: true + Length: 0.699999988079071 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: + Show Trail: false + Value: true + - Class: rviz/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 20 + Min Value: -3 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 239; 41; 41 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: currPoints + Position Transformer: XYZ + Queue Size: 100 + Selectable: true + Size (Pixels): 4 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_registered + Unreliable: false + Use Fixed Frame: true + Use rainbow: false + Value: true + - Alpha: 0.5 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 15 + Min Value: -5 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 238; 238; 236 + Color Transformer: Intensity + Decay Time: 100 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 159 + Min Color: 0; 0; 0 + Min Intensity: 5 + Name: surround + Position Transformer: XYZ + Queue Size: 1 + Selectable: false + Size (Pixels): 1 + Size (m): 0.004999999888241291 + Style: Points + Topic: /cloud_registered + Unreliable: true + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 239; 41; 41 + Color Transformer: "" + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 5 + Size (m): 0.019999999552965164 + Style: Squares + Topic: /cloud_effected + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + Enabled: true + Name: mapping + - Class: rviz/Group + Displays: + - Angle Tolerance: 0.009999999776482582 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 1 + Name: Odometry + Position Tolerance: 0.0010000000474974513 + Queue Size: 10 + Shape: + Alpha: 1 + Axes Length: 0.5 + Axes Radius: 0.15000000596046448 + Color: 255; 85; 0 + Head Length: 0 + Head Radius: 0 + Shaft Length: 0.800000011920929 + Shaft Radius: 0.5 + Value: Axes + Topic: /aft_mapped_to_init + Unreliable: false + Value: true + Enabled: true + Name: Odometry + - Alpha: 0 + Buffer Length: 2 + Class: rviz/Path + Color: 25; 255; 255 + Enabled: true + Head Diameter: 0 + Head Length: 0 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.03999999910593033 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 25; 255; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.4000000059604645 + Shaft Length: 0.4000000059604645 + Topic: /path + Unreliable: false + Value: true + - Alpha: 0.10000000149011612 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 20 + Min Value: -3 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 239; 41; 41 + Color Transformer: Intensity + Decay Time: 1000 + Enabled: false + Invert Rainbow: true + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: currPoints + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 2 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_voxel + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /planner_normal + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /voxels + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 245; 121; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 15 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_ray_sub_map + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /visualization_marker + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 92; 53; 102 + Color Transformer: FlatColor + Decay Time: 99999 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 10 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_visual_map + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 115; 210; 22 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: surround + Position Transformer: XYZ + Queue Size: 1 + Selectable: false + Size (Pixels): 12 + Size (m): 0.05000000074505806 + Style: Points + Topic: /cloud_visual_sub_map + Unreliable: true + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 20 + Min Value: -3 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 237; 212; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: currPoints + Position Transformer: XYZ + Queue Size: 100000 + Selectable: true + Size (Pixels): 5 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_sample_points + Unreliable: false + Use Fixed Frame: true + Use rainbow: false + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 239; 41; 41 + Color Transformer: FlatColor + Decay Time: 99999 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 239; 41; 41 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 4 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_visual_map + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 92; 53; 102 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 20 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_ray_sub_map_fov + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Angle Tolerance: 0 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: false + Keep: 1 + Name: Odometry + Position Tolerance: 0 + Queue Size: 10 + Shape: + Alpha: 1 + Axes Length: 0.699999988079071 + Axes Radius: 0.20000000298023224 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Axes + Topic: /aft_mapped_to_init + Unreliable: false + Value: false + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /waypoint_planner/visualize + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /fsm_node/visualization/exp_traj + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /fsm_node/visualization/exp_sfcs + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Image + Enabled: true + Image Topic: /camera/color/image_raw + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /camera/color/image_raw + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: false + - Class: rviz/Image + Enabled: true + Image Topic: /camera/color/segmentation_image + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 0; 0; 0 + Default Light: true + Fixed Frame: camera_init + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/ThirdPersonFollower + Distance: 7.5756306648254395 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 0.23840096592903137 + Y: -3.2277991771698 + Z: 0 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: true + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.670202374458313 + Target Frame: drone + Yaw: 1.3503977060317993 + Saved: + - Class: rviz/Orbit + Distance: 117.53474426269531 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: -35.713138580322266 + Y: 36.932613372802734 + Z: 4.459701061248779 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: far1 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.19539840519428253 + Target Frame: + Yaw: 0.17540442943572998 + - Class: rviz/Orbit + Distance: 109.3125 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: -22.092714309692383 + Y: 63.322662353515625 + Z: 14.125411987304688 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: far2 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.035398442298173904 + Target Frame: + Yaw: 5.793589115142822 + - Class: rviz/Orbit + Distance: 85.65605163574219 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 28.252656936645508 + Y: -35.49672317504883 + Z: -36.31112289428711 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: near1 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5653983950614929 + Target Frame: + Yaw: 0.9104044437408447 + - Class: rviz/Orbit + Distance: 60.1053581237793 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 30.61589241027832 + Y: 29.98663330078125 + Z: -12.290168762207031 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: near2 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.315398633480072 + Target Frame: + Yaw: 5.788588047027588 +Window Geometry: + Displays: + collapsed: false + Height: 1016 + Hide Left Dock: false + Hide Right Dock: false + Image: + collapsed: false + QMainWindow State: 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 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1848 + X: 72 + Y: 27 diff --git a/rviz_cfg/urbanloco.rviz b/rviz_cfg/urbanloco.rviz new file mode 100644 index 00000000..884b5430 --- /dev/null +++ b/rviz_cfg/urbanloco.rviz @@ -0,0 +1,684 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Status1 + - /Axes1 + - /mapping1 + - /mapping1/currPoints1 + - /mapping1/surround1 + - /mapping1/surround1/Autocompute Value Bounds1 + - /mapping1/PointCloud21 + - /Odometry1 + - /Odometry1/Odometry1 + - /Odometry1/Odometry1/Shape1 + - /Path1 + - /currPoints1/Autocompute Value Bounds1 + - /MarkerArray1/Namespaces1 + - /currPoints2/Autocompute Value Bounds1 + - /Odometry2/Shape1 + - /MarkerArray3 + - /MarkerArray4 + - /MarkerArray5 + - /Image1 + - /Image2 + Splitter Ratio: 0.34272301197052 + Tree Height: 360 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Name: Time + SyncMode: 0 + SyncSource: Image +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 1 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: false + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 160 + Reference Frame: + Value: false + - Alpha: 1 + Class: rviz/Axes + Enabled: true + Length: 0.699999988079071 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: + Show Trail: false + Value: true + - Class: rviz/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 20 + Min Value: -3 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 239; 41; 41 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: currPoints + Position Transformer: XYZ + Queue Size: 100 + Selectable: true + Size (Pixels): 4 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_registered + Unreliable: false + Use Fixed Frame: true + Use rainbow: false + Value: true + - Alpha: 0.5 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 15 + Min Value: -5 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 238; 238; 236 + Color Transformer: Intensity + Decay Time: 100 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 159 + Min Color: 0; 0; 0 + Min Intensity: 5 + Name: surround + Position Transformer: XYZ + Queue Size: 1 + Selectable: false + Size (Pixels): 1 + Size (m): 0.004999999888241291 + Style: Points + Topic: /cloud_registered + Unreliable: true + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 239; 41; 41 + Color Transformer: "" + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 5 + Size (m): 0.019999999552965164 + Style: Squares + Topic: /cloud_effected + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + Enabled: true + Name: mapping + - Class: rviz/Group + Displays: + - Angle Tolerance: 0.009999999776482582 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 1 + Name: Odometry + Position Tolerance: 0.0010000000474974513 + Queue Size: 10 + Shape: + Alpha: 1 + Axes Length: 0.5 + Axes Radius: 0.15000000596046448 + Color: 255; 85; 0 + Head Length: 0 + Head Radius: 0 + Shaft Length: 0.800000011920929 + Shaft Radius: 0.5 + Value: Axes + Topic: /aft_mapped_to_init + Unreliable: false + Value: true + Enabled: true + Name: Odometry + - Alpha: 0 + Buffer Length: 2 + Class: rviz/Path + Color: 25; 255; 255 + Enabled: true + Head Diameter: 0 + Head Length: 0 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.03999999910593033 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 25; 255; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.4000000059604645 + Shaft Length: 0.4000000059604645 + Topic: /path + Unreliable: false + Value: true + - Alpha: 0.10000000149011612 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 20 + Min Value: -3 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 239; 41; 41 + Color Transformer: Intensity + Decay Time: 1000 + Enabled: false + Invert Rainbow: true + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: currPoints + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 2 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_voxel + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /planner_normal + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /voxels + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 245; 121; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 15 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_ray_sub_map + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /visualization_marker + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 92; 53; 102 + Color Transformer: FlatColor + Decay Time: 99999 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 10 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_visual_map + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 115; 210; 22 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: surround + Position Transformer: XYZ + Queue Size: 1 + Selectable: false + Size (Pixels): 12 + Size (m): 0.05000000074505806 + Style: Points + Topic: /cloud_visual_sub_map + Unreliable: true + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 20 + Min Value: -3 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 237; 212; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: currPoints + Position Transformer: XYZ + Queue Size: 100000 + Selectable: true + Size (Pixels): 5 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_sample_points + Unreliable: false + Use Fixed Frame: true + Use rainbow: false + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 239; 41; 41 + Color Transformer: FlatColor + Decay Time: 99999 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 239; 41; 41 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 4 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_visual_map + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 92; 53; 102 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 20 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_ray_sub_map_fov + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Angle Tolerance: 0 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: false + Keep: 1 + Name: Odometry + Position Tolerance: 0 + Queue Size: 10 + Shape: + Alpha: 1 + Axes Length: 0.699999988079071 + Axes Radius: 0.20000000298023224 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Axes + Topic: /aft_mapped_to_init + Unreliable: false + Value: false + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /waypoint_planner/visualize + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /fsm_node/visualization/exp_traj + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /fsm_node/visualization/exp_sfcs + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Image + Enabled: true + Image Topic: /camera/color/image_raw + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /camera/color/image_raw + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: false + Enabled: true + Global Options: + Background Color: 0; 0; 0 + Default Light: true + Fixed Frame: camera_init + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/ThirdPersonFollower + Distance: 771.019287109375 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 38.612308502197266 + Y: 4.728164196014404 + Z: -5.862999023520388e-05 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: true + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: -1.5697963237762451 + Target Frame: drone + Yaw: 0.006785278208553791 + Saved: + - Class: rviz/Orbit + Distance: 117.53474426269531 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: -35.713138580322266 + Y: 36.932613372802734 + Z: 4.459701061248779 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: far1 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.19539840519428253 + Target Frame: + Yaw: 0.17540442943572998 + - Class: rviz/Orbit + Distance: 109.3125 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: -22.092714309692383 + Y: 63.322662353515625 + Z: 14.125411987304688 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: far2 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.035398442298173904 + Target Frame: + Yaw: 5.793589115142822 + - Class: rviz/Orbit + Distance: 85.65605163574219 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 28.252656936645508 + Y: -35.49672317504883 + Z: -36.31112289428711 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: near1 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5653983950614929 + Target Frame: + Yaw: 0.9104044437408447 + - Class: rviz/Orbit + Distance: 60.1053581237793 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 30.61589241027832 + Y: 29.98663330078125 + Z: -12.290168762207031 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: near2 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.315398633480072 + Target Frame: + Yaw: 5.788588047027588 +Window Geometry: + Displays: + collapsed: false + Height: 1016 + Hide Left Dock: false + Hide Right Dock: false + Image: + collapsed: false + QMainWindow State: 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 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1848 + X: 72 + Y: 27 diff --git a/rviz_cfg/urbanloco_ca.rviz b/rviz_cfg/urbanloco_ca.rviz new file mode 100644 index 00000000..1dc5df03 --- /dev/null +++ b/rviz_cfg/urbanloco_ca.rviz @@ -0,0 +1,205 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Grid1 + - /Axes2 + - /Group1 + - /Group1/CurrPointCloud1 + Splitter Ratio: 0.5 + Tree Height: 549 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Name: Time + SyncMode: 0 + SyncSource: CurrPointCloud +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: + Show Trail: false + Value: true + - Alpha: 1 + Class: rviz/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: aft_mapped + Show Trail: false + Value: true + - Class: rviz/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 239; 41; 41 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: CurrPointCloud + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.03999999910593033 + Style: Flat Squares + Topic: /cloud_registered + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Group + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /path + Unreliable: false + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /planner_normal + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: camera_init + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 71.97770690917969 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 14.618456840515137 + Y: 0.9966355562210083 + Z: -4.635408401489258 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5947967767715454 + Target Frame: + Yaw: 5.023595809936523 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 897 + Y: 323 diff --git a/scripts/calculate_transform.py b/scripts/calculate_transform.py new file mode 100644 index 00000000..fa054075 --- /dev/null +++ b/scripts/calculate_transform.py @@ -0,0 +1,19 @@ +import numpy as np + + +T_camera_lidar = np.array( + [ 0, 0, 1, + -1, 0, 0, + 0, -1, 0] +) +T_camera_lidar = T_camera_lidar.reshape(3, 3) + +T_lidar_imu = np.array( + [ 0, -1, 0, + 1, 0, 0, + 0, 0, 1] +) +T_lidar_imu = T_lidar_imu.reshape(3, 3) + +T_camera_imu = T_camera_lidar @ T_lidar_imu +print(T_camera_imu) \ No newline at end of file diff --git a/scripts/camera_model.py b/scripts/camera_model.py new file mode 100644 index 00000000..88dd98d8 --- /dev/null +++ b/scripts/camera_model.py @@ -0,0 +1,110 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +import os +import yaml +import math +import numpy as np +import cv2 +import rospy +from cv_bridge import CvBridge +from sensor_msgs.msg import Image, CompressedImage + +class OmmiUndistortion: + def __init__(self): + self.init_params() + self.init_subscriber() + self.init_publisher() + + def init_params(self): + import numpy as np + import cv2 + from cv_bridge import CvBridge + + # ---- Intrinsics (omni/Mei) ---- + self.xi = float(1) + self.fx = float(1308.1691840205795) + self.fy = float(1318.683585974909) + self.cx = float(1221.8790961896898) + self.cy = float(1052.024852216732) + + # ---- Image size ---- + self.width = int(2448) + self.height = int(2048) + + # ---- I/O ---- + self.bridge = CvBridge() + + # ---- Camera matrix & distortion (radtan: k1,k2,p1,p2) ---- + self.K = np.array([[self.fx, 0.0, self.cx], + [0.0, self.fy, self.cy], + [0.0, 0.0, 1.0]], dtype=np.float64) + + self.D = np.array( + [-0.13274940732954246, 0.3435168982763474, + 0.007927480590485703, -0.00010939869901162644], + dtype=np.float64 + ).reshape(-1) # (4,) + + # ---- Projection matrix (Knew): giữ FOV gốc, nhưng tâm ảnh đặt giữa khung mới ---- + Knew = self.K.copy() + Knew[0, 2] = self.width / 2.0 # cx_new + Knew[1, 2] = self.height / 2.0 # cy_new + Knew = Knew.astype(np.float64) + + # (Tuỳ chọn) Nếu muốn FOV ~90° thay vì giữ FOV gốc, bỏ comment đoạn dưới: + # import math + # f = 0.5 * self.width / math.tan(math.radians(90.0) * 0.5) + # Knew = np.array([[f, 0, self.width/2.0], + # [0, f, self.height/2.0], + # [0, 0, 1]], dtype=np.float64) + + # ---- Rotation (view direction). Nếu không cần xoay, dùng identity float64 hoặc None ---- + self.R = np.eye(3, dtype=np.float64) # hoặc: self.R = None + + # ---- Precompute remap (lưu ý: tham số thứ 7 là mltype, không phải m1type) ---- + # Size phải là (width, height) + xi_scalar = np.array([self.xi], dtype=np.float64) + + self.mapx, self.mapy = cv2.omnidir.initUndistortRectifyMap( + self.K, # CV_64F, 3x3 + self.D, # (4,), CV_64F + xi_scalar, # scalar + self.R, # None hoặc 3x3 CV_32F/CV_64F + Knew, # projection matrix cho ảnh đầu ra + (self.width, self.height), + cv2.CV_32FC1, # <-- mltype (truyền theo vị trí) + flags=cv2.omnidir.RECTIFY_PERSPECTIVE + ) + + def init_publisher(self): + self.img_pub = rospy.Publisher("/camera/image_undirtortion", Image, queue_size=1) + + + def init_subscriber(self): + self.ommi_sub = rospy.Subscriber("/camera/image_color", Image, self.ommi_handler, queue_size= 1) + + def rectify(self, bgr): + return cv2.remap(bgr, self.mapx, self.mapy, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT) + + + def ommi_handler(self, msg: Image): + print("[Ommi Handle] received image.") + try: + bgr = self.bridge.imgmsg_to_cv2(msg, "bgr8") + rect = self.rectify(bgr) + + out = self.bridge.cv2_to_imgmsg(rect, "bgr8"); out.header = msg.header + self.img_pub.publish(out) + + except Exception as e: + rospy.logwarn("cb_image error: %s", e) + + +def main(): + rospy.init_node("omnidir_undistort_node") + OmmiUndistortion() + rospy.spin() + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/src/LIVMapper.cpp b/src/LIVMapper.cpp index 5e6861b3..8eac5400 100755 --- a/src/LIVMapper.cpp +++ b/src/LIVMapper.cpp @@ -1,4 +1,4 @@ -/* +/* This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. Developer: Chunran Zheng @@ -16,1292 +16,1595 @@ LIVMapper::LIVMapper(ros::NodeHandle &nh) : extT(0, 0, 0), extR(M3D::Identity()) { - extrinT.assign(3, 0.0); - extrinR.assign(9, 0.0); - cameraextrinT.assign(3, 0.0); - cameraextrinR.assign(9, 0.0); - - p_pre.reset(new Preprocess()); - p_imu.reset(new ImuProcess()); - - readParameters(nh); - VoxelMapConfig voxel_config; - loadVoxelConfig(nh, voxel_config); - - visual_sub_map.reset(new PointCloudXYZI()); - feats_undistort.reset(new PointCloudXYZI()); - feats_down_body.reset(new PointCloudXYZI()); - feats_down_world.reset(new PointCloudXYZI()); - pcl_w_wait_pub.reset(new PointCloudXYZI()); - pcl_wait_pub.reset(new PointCloudXYZI()); - pcl_wait_save.reset(new PointCloudXYZRGB()); - pcl_wait_save_intensity.reset(new PointCloudXYZI()); - voxelmap_manager.reset(new VoxelMapManager(voxel_config, voxel_map)); - vio_manager.reset(new VIOManager()); - root_dir = ROOT_DIR; - initializeFiles(); - initializeComponents(); - path.header.stamp = ros::Time::now(); - path.header.frame_id = "camera_init"; + extrinT.assign(3, 0.0); + extrinR.assign(9, 0.0); + cameraextrinT.assign(3, 0.0); + cameraextrinR.assign(9, 0.0); + + p_pre.reset(new Preprocess()); + p_imu.reset(new ImuProcess()); + + readParameters(nh); + VoxelMapConfig voxel_config; + loadVoxelConfig(nh, voxel_config); + + visual_sub_map.reset(new PointCloudXYZI()); + feats_undistort.reset(new PointCloudXYZI()); + feats_down_body.reset(new PointCloudXYZI()); + feats_down_world.reset(new PointCloudXYZI()); + pcl_w_wait_pub.reset(new PointCloudXYZI()); + pcl_wait_pub.reset(new PointCloudXYZI()); + pcl_wait_save.reset(new PointCloudXYZRGB()); + pcl_wait_save_intensity.reset(new PointCloudXYZI()); + voxelmap_manager.reset(new VoxelMapManager(voxel_config, voxel_map)); + vio_manager.reset(new VIOManager()); + root_dir = ROOT_DIR; + initializeFiles(); + initializeComponents(); + path.header.stamp = ros::Time::now(); + path.header.frame_id = "camera_init"; } LIVMapper::~LIVMapper() {} void LIVMapper::readParameters(ros::NodeHandle &nh) { - nh.param("common/lid_topic", lid_topic, "/livox/lidar"); - nh.param("common/imu_topic", imu_topic, "/livox/imu"); - nh.param("common/ros_driver_bug_fix", ros_driver_fix_en, false); - nh.param("common/img_en", img_en, 1); - nh.param("common/lidar_en", lidar_en, 1); - nh.param("common/img_topic", img_topic, "/left_camera/image"); - - nh.param("vio/normal_en", normal_en, true); - nh.param("vio/inverse_composition_en", inverse_composition_en, false); - nh.param("vio/max_iterations", max_iterations, 5); - nh.param("vio/img_point_cov", IMG_POINT_COV, 100); - nh.param("vio/raycast_en", raycast_en, false); - nh.param("vio/exposure_estimate_en", exposure_estimate_en, true); - nh.param("vio/inv_expo_cov", inv_expo_cov, 0.2); - nh.param("vio/grid_size", grid_size, 5); - nh.param("vio/grid_n_height", grid_n_height, 17); - nh.param("vio/patch_pyrimid_level", patch_pyrimid_level, 3); - nh.param("vio/patch_size", patch_size, 8); - nh.param("vio/outlier_threshold", outlier_threshold, 1000); - - nh.param("time_offset/exposure_time_init", exposure_time_init, 0.0); - nh.param("time_offset/img_time_offset", img_time_offset, 0.0); - nh.param("time_offset/imu_time_offset", imu_time_offset, 0.0); - nh.param("time_offset/lidar_time_offset", lidar_time_offset, 0.0); - nh.param("uav/imu_rate_odom", imu_prop_enable, false); - nh.param("uav/gravity_align_en", gravity_align_en, false); - - nh.param("evo/seq_name", seq_name, "01"); - nh.param("evo/pose_output_en", pose_output_en, false); - nh.param("imu/gyr_cov", gyr_cov, 1.0); - nh.param("imu/acc_cov", acc_cov, 1.0); - nh.param("imu/imu_int_frame", imu_int_frame, 3); - nh.param("imu/imu_en", imu_en, false); - nh.param("imu/gravity_est_en", gravity_est_en, true); - nh.param("imu/ba_bg_est_en", ba_bg_est_en, true); - - nh.param("preprocess/blind", p_pre->blind, 0.01); - nh.param("preprocess/filter_size_surf", filter_size_surf_min, 0.5); - nh.param("preprocess/hilti_en", hilti_en, false); - nh.param("preprocess/lidar_type", p_pre->lidar_type, AVIA); - nh.param("preprocess/scan_line", p_pre->N_SCANS, 6); - nh.param("preprocess/point_filter_num", p_pre->point_filter_num, 3); - nh.param("preprocess/feature_extract_enabled", p_pre->feature_enabled, false); - - nh.param("pcd_save/interval", pcd_save_interval, -1); - nh.param("pcd_save/pcd_save_en", pcd_save_en, false); - nh.param("pcd_save/colmap_output_en", colmap_output_en, false); - nh.param("pcd_save/filter_size_pcd", filter_size_pcd, 0.5); - nh.param>("extrin_calib/extrinsic_T", extrinT, vector()); - nh.param>("extrin_calib/extrinsic_R", extrinR, vector()); - nh.param>("extrin_calib/Pcl", cameraextrinT, vector()); - nh.param>("extrin_calib/Rcl", cameraextrinR, vector()); - nh.param("debug/plot_time", plot_time, -10); - nh.param("debug/frame_cnt", frame_cnt, 6); - - nh.param("publish/blind_rgb_points", blind_rgb_points, 0.01); - nh.param("publish/pub_scan_num", pub_scan_num, 1); - nh.param("publish/pub_effect_point_en", pub_effect_point_en, false); - nh.param("publish/dense_map_en", dense_map_en, false); - - p_pre->blind_sqr = p_pre->blind * p_pre->blind; + nh.param("common/lid_topic", lid_topic, "/livox/lidar"); + nh.param("common/imu_topic", imu_topic, "/livox/imu"); + nh.param("common/mask_topic", mask_topic, "/camera/color/segmentation_image"); + nh.param("common/ros_driver_bug_fix", ros_driver_fix_en, false); + nh.param("common/img_en", img_en, 1); + nh.param("common/lidar_en", lidar_en, 1); + nh.param("common/img_topic", img_topic, "/left_camera/image"); + nh.param("common/is_compressed_image", is_compressed_image, false); + nh.param("common/dynamic_slam", dynamic_slam, false); + nh.param("vio/normal_en", normal_en, true); + nh.param("vio/inverse_composition_en", inverse_composition_en, false); + nh.param("vio/max_iterations", max_iterations, 5); + nh.param("vio/img_point_cov", IMG_POINT_COV, 100); + nh.param("vio/raycast_en", raycast_en, false); + nh.param("vio/exposure_estimate_en", exposure_estimate_en, true); + nh.param("vio/inv_expo_cov", inv_expo_cov, 0.2); + nh.param("vio/grid_size", grid_size, 5); + nh.param("vio/grid_n_height", grid_n_height, 17); + nh.param("vio/patch_pyrimid_level", patch_pyrimid_level, 3); + nh.param("vio/patch_size", patch_size, 8); + nh.param("vio/outlier_threshold", outlier_threshold, 1000); + + nh.param("time_offset/exposure_time_init", exposure_time_init, 0.0); + nh.param("time_offset/img_time_offset", img_time_offset, 0.0); + nh.param("time_offset/imu_time_offset", imu_time_offset, 0.0); + nh.param("time_offset/lidar_time_offset", lidar_time_offset, 0.0); + nh.param("uav/imu_rate_odom", imu_prop_enable, false); + nh.param("uav/gravity_align_en", gravity_align_en, false); + + nh.param("evo/seq_name", seq_name, "01"); + nh.param("evo/pose_output_en", pose_output_en, false); + nh.param("imu/gyr_cov", gyr_cov, 1.0); + nh.param("imu/acc_cov", acc_cov, 1.0); + nh.param("imu/imu_int_frame", imu_int_frame, 3); + nh.param("imu/imu_en", imu_en, false); + nh.param("imu/gravity_est_en", gravity_est_en, true); + nh.param("imu/ba_bg_est_en", ba_bg_est_en, true); + + nh.param("preprocess/blind", p_pre->blind, 0.01); + nh.param("preprocess/filter_size_surf", filter_size_surf_min, 0.5); + nh.param("preprocess/hilti_en", hilti_en, false); + nh.param("preprocess/lidar_type", p_pre->lidar_type, AVIA); + nh.param("preprocess/scan_line", p_pre->N_SCANS, 6); + nh.param("preprocess/point_filter_num", p_pre->point_filter_num, 3); + nh.param("preprocess/feature_extract_enabled", p_pre->feature_enabled, false); + + nh.param("pcd_save/interval", pcd_save_interval, -1); + nh.param("pcd_save/pcd_save_en", pcd_save_en, false); + nh.param("pcd_save/colmap_output_en", colmap_output_en, false); + nh.param("pcd_save/filter_size_pcd", filter_size_pcd, 0.5); + nh.param>("extrin_calib/extrinsic_T", extrinT, vector()); + nh.param>("extrin_calib/extrinsic_R", extrinR, vector()); + nh.param>("extrin_calib/Pcl", cameraextrinT, vector()); + nh.param>("extrin_calib/Rcl", cameraextrinR, vector()); + nh.param("debug/plot_time", plot_time, -10); + nh.param("debug/frame_cnt", frame_cnt, 6); + + nh.param("publish/blind_rgb_points", blind_rgb_points, 0.01); + nh.param("publish/pub_scan_num", pub_scan_num, 1); + nh.param("publish/pub_effect_point_en", pub_effect_point_en, false); + nh.param("publish/dense_map_en", dense_map_en, false); + + p_pre->blind_sqr = p_pre->blind * p_pre->blind; } -void LIVMapper::initializeComponents() +void LIVMapper::initializeComponents() { - downSizeFilterSurf.setLeafSize(filter_size_surf_min, filter_size_surf_min, filter_size_surf_min); - extT << VEC_FROM_ARRAY(extrinT); - extR << MAT_FROM_ARRAY(extrinR); - - voxelmap_manager->extT_ << VEC_FROM_ARRAY(extrinT); - voxelmap_manager->extR_ << MAT_FROM_ARRAY(extrinR); - - if (!vk::camera_loader::loadFromRosNs("laserMapping", vio_manager->cam)) throw std::runtime_error("Camera model not correctly specified."); - - vio_manager->grid_size = grid_size; - vio_manager->patch_size = patch_size; - vio_manager->outlier_threshold = outlier_threshold; - vio_manager->setImuToLidarExtrinsic(extT, extR); - vio_manager->setLidarToCameraExtrinsic(cameraextrinR, cameraextrinT); - vio_manager->state = &_state; - vio_manager->state_propagat = &state_propagat; - vio_manager->max_iterations = max_iterations; - vio_manager->img_point_cov = IMG_POINT_COV; - vio_manager->normal_en = normal_en; - vio_manager->inverse_composition_en = inverse_composition_en; - vio_manager->raycast_en = raycast_en; - vio_manager->grid_n_width = grid_n_width; - vio_manager->grid_n_height = grid_n_height; - vio_manager->patch_pyrimid_level = patch_pyrimid_level; - vio_manager->exposure_estimate_en = exposure_estimate_en; - vio_manager->colmap_output_en = colmap_output_en; - vio_manager->initializeVIO(); - - p_imu->set_extrinsic(extT, extR); - p_imu->set_gyr_cov_scale(V3D(gyr_cov, gyr_cov, gyr_cov)); - p_imu->set_acc_cov_scale(V3D(acc_cov, acc_cov, acc_cov)); - p_imu->set_inv_expo_cov(inv_expo_cov); - p_imu->set_gyr_bias_cov(V3D(0.0001, 0.0001, 0.0001)); - p_imu->set_acc_bias_cov(V3D(0.0001, 0.0001, 0.0001)); - p_imu->set_imu_init_frame_num(imu_int_frame); - - if (!imu_en) p_imu->disable_imu(); - if (!gravity_est_en) p_imu->disable_gravity_est(); - if (!ba_bg_est_en) p_imu->disable_bias_est(); - if (!exposure_estimate_en) p_imu->disable_exposure_est(); - - slam_mode_ = (img_en && lidar_en) ? LIVO : imu_en ? ONLY_LIO : ONLY_LO; + downSizeFilterSurf.setLeafSize(filter_size_surf_min, filter_size_surf_min, filter_size_surf_min); + extT << VEC_FROM_ARRAY(extrinT); + extR << MAT_FROM_ARRAY(extrinR); + + voxelmap_manager->extT_ << VEC_FROM_ARRAY(extrinT); + voxelmap_manager->extR_ << MAT_FROM_ARRAY(extrinR); + + if (!vk::camera_loader::loadFromRosNs("laserMapping", vio_manager->cam)) + throw std::runtime_error("Camera model not correctly specified."); + + vio_manager->grid_size = grid_size; + vio_manager->patch_size = patch_size; + vio_manager->outlier_threshold = outlier_threshold; + vio_manager->setImuToLidarExtrinsic(extT, extR); + vio_manager->setLidarToCameraExtrinsic(cameraextrinR, cameraextrinT); + vio_manager->state = &_state; + vio_manager->state_propagat = &state_propagat; + vio_manager->max_iterations = max_iterations; + vio_manager->img_point_cov = IMG_POINT_COV; + vio_manager->normal_en = normal_en; + vio_manager->inverse_composition_en = inverse_composition_en; + vio_manager->raycast_en = raycast_en; + vio_manager->grid_n_width = grid_n_width; + vio_manager->grid_n_height = grid_n_height; + vio_manager->patch_pyrimid_level = patch_pyrimid_level; + vio_manager->exposure_estimate_en = exposure_estimate_en; + vio_manager->colmap_output_en = colmap_output_en; + vio_manager->initializeVIO(); + + p_imu->set_extrinsic(extT, extR); + p_imu->set_gyr_cov_scale(V3D(gyr_cov, gyr_cov, gyr_cov)); + p_imu->set_acc_cov_scale(V3D(acc_cov, acc_cov, acc_cov)); + p_imu->set_inv_expo_cov(inv_expo_cov); + p_imu->set_gyr_bias_cov(V3D(0.0001, 0.0001, 0.0001)); + p_imu->set_acc_bias_cov(V3D(0.0001, 0.0001, 0.0001)); + p_imu->set_imu_init_frame_num(imu_int_frame); + + if (!imu_en) + p_imu->disable_imu(); + if (!gravity_est_en) + p_imu->disable_gravity_est(); + if (!ba_bg_est_en) + p_imu->disable_bias_est(); + if (!exposure_estimate_en) + p_imu->disable_exposure_est(); + + slam_mode_ = (img_en && lidar_en) ? LIVO : imu_en ? ONLY_LIO + : ONLY_LO; } -void LIVMapper::initializeFiles() +void LIVMapper::initializeFiles() { - if (pcd_save_en && colmap_output_en) - { - const std::string folderPath = std::string(ROOT_DIR) + "/scripts/colmap_output.sh"; - - std::string chmodCommand = "chmod +x " + folderPath; - - int chmodRet = system(chmodCommand.c_str()); - if (chmodRet != 0) { - std::cerr << "Failed to set execute permissions for the script." << std::endl; - return; - } - - int executionRet = system(folderPath.c_str()); - if (executionRet != 0) { - std::cerr << "Failed to execute the script." << std::endl; - return; - } - } - if(colmap_output_en) fout_points.open(std::string(ROOT_DIR) + "Log/Colmap/sparse/0/points3D.txt", std::ios::out); - if(pcd_save_interval > 0) fout_pcd_pos.open(std::string(ROOT_DIR) + "Log/PCD/scans_pos.json", std::ios::out); - fout_pre.open(DEBUG_FILE_DIR("mat_pre.txt"), std::ios::out); - fout_out.open(DEBUG_FILE_DIR("mat_out.txt"), std::ios::out); + if (pcd_save_en && colmap_output_en) + { + const std::string folderPath = std::string(ROOT_DIR) + "/scripts/colmap_output.sh"; + + std::string chmodCommand = "chmod +x " + folderPath; + + int chmodRet = system(chmodCommand.c_str()); + if (chmodRet != 0) + { + std::cerr << "Failed to set execute permissions for the script." << std::endl; + return; + } + + int executionRet = system(folderPath.c_str()); + if (executionRet != 0) + { + std::cerr << "Failed to execute the script." << std::endl; + return; + } + } + if (colmap_output_en) + fout_points.open(std::string(ROOT_DIR) + "Log/Colmap/sparse/0/points3D.txt", std::ios::out); + if (pcd_save_interval > 0) + fout_pcd_pos.open(std::string(ROOT_DIR) + "Log/PCD/scans_pos.json", std::ios::out); + fout_pre.open(DEBUG_FILE_DIR("mat_pre.txt"), std::ios::out); + fout_out.open(DEBUG_FILE_DIR("mat_out.txt"), std::ios::out); } -void LIVMapper::initializeSubscribersAndPublishers(ros::NodeHandle &nh, image_transport::ImageTransport &it) +void LIVMapper::initializeSubscribersAndPublishers(ros::NodeHandle &nh, image_transport::ImageTransport &it) { - sub_pcl = p_pre->lidar_type == AVIA ? - nh.subscribe(lid_topic, 200000, &LIVMapper::livox_pcl_cbk, this): - nh.subscribe(lid_topic, 200000, &LIVMapper::standard_pcl_cbk, this); - sub_imu = nh.subscribe(imu_topic, 200000, &LIVMapper::imu_cbk, this); - sub_img = nh.subscribe(img_topic, 200000, &LIVMapper::img_cbk, this); - - pubLaserCloudFullRes = nh.advertise("/cloud_registered", 100); - pubNormal = nh.advertise("visualization_marker", 100); - pubSubVisualMap = nh.advertise("/cloud_visual_sub_map_before", 100); - pubLaserCloudEffect = nh.advertise("/cloud_effected", 100); - pubLaserCloudMap = nh.advertise("/Laser_map", 100); - pubOdomAftMapped = nh.advertise("/aft_mapped_to_init", 10); - pubPath = nh.advertise("/path", 10); - plane_pub = nh.advertise("/planner_normal", 1); - voxel_pub = nh.advertise("/voxels", 1); - pubLaserCloudDyn = nh.advertise("/dyn_obj", 100); - pubLaserCloudDynRmed = nh.advertise("/dyn_obj_removed", 100); - pubLaserCloudDynDbg = nh.advertise("/dyn_obj_dbg_hist", 100); - mavros_pose_publisher = nh.advertise("/mavros/vision_pose/pose", 10); - pubImage = it.advertise("/rgb_img", 1); - pubImuPropOdom = nh.advertise("/LIVO2/imu_propagate", 10000); - imu_prop_timer = nh.createTimer(ros::Duration(0.004), &LIVMapper::imu_prop_callback, this); - voxelmap_manager->voxel_map_pub_= nh.advertise("/planes", 10000); + sub_pcl = p_pre->lidar_type == AVIA ? nh.subscribe(lid_topic, 200000, &LIVMapper::livox_pcl_cbk, this) : nh.subscribe(lid_topic, 200000, &LIVMapper::standard_pcl_cbk, this); + sub_imu = nh.subscribe(imu_topic, 200000, &LIVMapper::imu_cbk, this); + + if (dynamic_slam) + { + std::cout << "[LIVMapper::initializeSubscribersAndPublishers] using dynamic slam. " << std::endl; + if(is_compressed_image) + { + std::cout << "[LIVMapper::initializeSubscribersAndPublishers] compressed image: " << img_topic << std::endl; + std::cout << "[LIVMapper::initializeSubscribersAndPublishers] mask topic: " << mask_topic << std::endl; + message_filters::Subscriber img_sub(nh, img_topic, 100); + message_filters::Subscriber mask_sub(nh, mask_topic, 100); + com_sync = std::make_unique>( + CompressedSyncPolicy(10), img_sub, mask_sub); + + com_sync->registerCallback(boost::bind(&LIVMapper::compressed_mask_cbk, this, _1, _2)); + } + else + { + std::cout << "[LIVMapper::initializeSubscribersAndPublishers] using image msg. " << std::endl; + message_filters::Subscriber img_sub(nh, img_topic, 100); + message_filters::Subscriber mask_sub(nh, mask_topic, 100); + img_sync = std::make_unique>( + ImageSyncPolicy(10), img_sub, mask_sub); + + img_sync->registerCallback(boost::bind(&LIVMapper::img_mask_cbk, this, _1, _2)); + } + + } + + else + { + if (is_compressed_image) + { + sub_img = nh.subscribe(img_topic, 200000, &LIVMapper::compressed_cbk, this); + } + else + { + sub_img = nh.subscribe(img_topic, 200000, &LIVMapper::img_cbk, this); + } + } + + + pubLaserCloudFullRes = nh.advertise("/cloud_registered", 100); + pubNormal = nh.advertise("visualization_marker", 100); + pubSubVisualMap = nh.advertise("/cloud_visual_sub_map_before", 100); + pubLaserCloudEffect = nh.advertise("/cloud_effected", 100); + pubLaserCloudMap = nh.advertise("/Laser_map", 100); + pubOdomAftMapped = nh.advertise("/aft_mapped_to_init", 10); + pubPath = nh.advertise("/path", 10); + plane_pub = nh.advertise("/planner_normal", 1); + voxel_pub = nh.advertise("/voxels", 1); + pubLaserCloudDyn = nh.advertise("/dyn_obj", 100); + pubLaserCloudDynRmed = nh.advertise("/dyn_obj_removed", 100); + pubLaserCloudDynDbg = nh.advertise("/dyn_obj_dbg_hist", 100); + mavros_pose_publisher = nh.advertise("/mavros/vision_pose/pose", 10); + pubImage = it.advertise("/rgb_img", 1); + pubImuPropOdom = nh.advertise("/LIVO2/imu_propagate", 10000); + imu_prop_timer = nh.createTimer(ros::Duration(0.004), &LIVMapper::imu_prop_callback, this); + voxelmap_manager->voxel_map_pub_ = nh.advertise("/planes", 10000); } -void LIVMapper::handleFirstFrame() +void LIVMapper::handleFirstFrame() { - if (!is_first_frame) - { - _first_lidar_time = LidarMeasures.last_lio_update_time; - p_imu->first_lidar_time = _first_lidar_time; // Only for IMU data log - is_first_frame = true; - cout << "FIRST LIDAR FRAME!" << endl; - } + if (!is_first_frame) + { + _first_lidar_time = LidarMeasures.last_lio_update_time; + p_imu->first_lidar_time = _first_lidar_time; // Only for IMU data log + is_first_frame = true; + cout << "FIRST LIDAR FRAME!" << endl; + } } -void LIVMapper::gravityAlignment() +void LIVMapper::gravityAlignment() { - if (!p_imu->imu_need_init && !gravity_align_finished) - { - std::cout << "Gravity Alignment Starts" << std::endl; - V3D ez(0, 0, -1), gz(_state.gravity); - Quaterniond G_q_I0 = Quaterniond::FromTwoVectors(gz, ez); - M3D G_R_I0 = G_q_I0.toRotationMatrix(); - - _state.pos_end = G_R_I0 * _state.pos_end; - _state.rot_end = G_R_I0 * _state.rot_end; - _state.vel_end = G_R_I0 * _state.vel_end; - _state.gravity = G_R_I0 * _state.gravity; - gravity_align_finished = true; - std::cout << "Gravity Alignment Finished" << std::endl; - } + if (!p_imu->imu_need_init && !gravity_align_finished) + { + std::cout << "Gravity Alignment Starts" << std::endl; + V3D ez(0, 0, -1), gz(_state.gravity); + Quaterniond G_q_I0 = Quaterniond::FromTwoVectors(gz, ez); + M3D G_R_I0 = G_q_I0.toRotationMatrix(); + + _state.pos_end = G_R_I0 * _state.pos_end; + _state.rot_end = G_R_I0 * _state.rot_end; + _state.vel_end = G_R_I0 * _state.vel_end; + _state.gravity = G_R_I0 * _state.gravity; + gravity_align_finished = true; + std::cout << "Gravity Alignment Finished" << std::endl; + } } -void LIVMapper::processImu() +void LIVMapper::processImu() { - // double t0 = omp_get_wtime(); + // double t0 = omp_get_wtime(); - p_imu->Process2(LidarMeasures, _state, feats_undistort); + p_imu->Process2(LidarMeasures, _state, feats_undistort); - if (gravity_align_en) gravityAlignment(); + if (gravity_align_en) + gravityAlignment(); - state_propagat = _state; - voxelmap_manager->state_ = _state; - voxelmap_manager->feats_undistort_ = feats_undistort; + state_propagat = _state; + voxelmap_manager->state_ = _state; + voxelmap_manager->feats_undistort_ = feats_undistort; - // double t_prop = omp_get_wtime(); + // double t_prop = omp_get_wtime(); - // std::cout << "[ Mapping ] feats_undistort: " << feats_undistort->size() << std::endl; - // std::cout << "[ Mapping ] predict cov: " << _state.cov.diagonal().transpose() << std::endl; - // std::cout << "[ Mapping ] predict sta: " << state_propagat.pos_end.transpose() << state_propagat.vel_end.transpose() << std::endl; + // std::cout << "[ Mapping ] feats_undistort: " << feats_undistort->size() << std::endl; + // std::cout << "[ Mapping ] predict cov: " << _state.cov.diagonal().transpose() << std::endl; + // std::cout << "[ Mapping ] predict sta: " << state_propagat.pos_end.transpose() << state_propagat.vel_end.transpose() << std::endl; } -void LIVMapper::stateEstimationAndMapping() +void LIVMapper::stateEstimationAndMapping() { - switch (LidarMeasures.lio_vio_flg) - { + switch (LidarMeasures.lio_vio_flg) + { case VIO: - handleVIO(); - break; + handleVIO(); + break; case LIO: case LO: - handleLIO(); - break; - } + handleLIO(); + break; + } } -void LIVMapper::handleVIO() +void LIVMapper::handleVIO() { - euler_cur = RotMtoEuler(_state.rot_end); - fout_pre << std::setw(20) << LidarMeasures.last_lio_update_time - _first_lidar_time << " " << euler_cur.transpose() * 57.3 << " " - << _state.pos_end.transpose() << " " << _state.vel_end.transpose() << " " << _state.bias_g.transpose() << " " - << _state.bias_a.transpose() << " " << V3D(_state.inv_expo_time, 0, 0).transpose() << std::endl; - - if (pcl_w_wait_pub->empty() || (pcl_w_wait_pub == nullptr)) - { - std::cout << "[ VIO ] No point!!!" << std::endl; - return; - } - - std::cout << "[ VIO ] Raw feature num: " << pcl_w_wait_pub->points.size() << std::endl; - - if (fabs((LidarMeasures.last_lio_update_time - _first_lidar_time) - plot_time) < (frame_cnt / 2 * 0.1)) - { - vio_manager->plot_flag = true; - } - else - { - vio_manager->plot_flag = false; - } - - vio_manager->processFrame(LidarMeasures.measures.back().img, _pv_list, voxelmap_manager->voxel_map_, LidarMeasures.last_lio_update_time - _first_lidar_time); - - if (imu_prop_enable) - { - ekf_finish_once = true; - latest_ekf_state = _state; - latest_ekf_time = LidarMeasures.last_lio_update_time; - state_update_flg = true; - } - - // int size_sub_map = vio_manager->visual_sub_map_cur.size(); - // visual_sub_map->reserve(size_sub_map); - // for (int i = 0; i < size_sub_map; i++) - // { - // PointType temp_map; - // temp_map.x = vio_manager->visual_sub_map_cur[i]->pos_[0]; - // temp_map.y = vio_manager->visual_sub_map_cur[i]->pos_[1]; - // temp_map.z = vio_manager->visual_sub_map_cur[i]->pos_[2]; - // temp_map.intensity = 0.; - // visual_sub_map->push_back(temp_map); - // } - - publish_frame_world(pubLaserCloudFullRes, vio_manager); - publish_img_rgb(pubImage, vio_manager); - - euler_cur = RotMtoEuler(_state.rot_end); - fout_out << std::setw(20) << LidarMeasures.last_lio_update_time - _first_lidar_time << " " << euler_cur.transpose() * 57.3 << " " - << _state.pos_end.transpose() << " " << _state.vel_end.transpose() << " " << _state.bias_g.transpose() << " " - << _state.bias_a.transpose() << " " << V3D(_state.inv_expo_time, 0, 0).transpose() << " " << feats_undistort->points.size() << std::endl; -} + euler_cur = RotMtoEuler(_state.rot_end); + fout_pre << std::setw(20) << LidarMeasures.last_lio_update_time - _first_lidar_time << " " << euler_cur.transpose() * 57.3 << " " + << _state.pos_end.transpose() << " " << _state.vel_end.transpose() << " " << _state.bias_g.transpose() << " " + << _state.bias_a.transpose() << " " << V3D(_state.inv_expo_time, 0, 0).transpose() << std::endl; + + if (pcl_w_wait_pub->empty() || (pcl_w_wait_pub == nullptr)) + { + std::cout << "[ VIO ] No point!!!" << std::endl; + return; + } + + std::cout << "[ VIO ] Raw feature num: " << pcl_w_wait_pub->points.size() << std::endl; -void LIVMapper::handleLIO() -{ - euler_cur = RotMtoEuler(_state.rot_end); - fout_pre << setw(20) << LidarMeasures.last_lio_update_time - _first_lidar_time << " " << euler_cur.transpose() * 57.3 << " " - << _state.pos_end.transpose() << " " << _state.vel_end.transpose() << " " << _state.bias_g.transpose() << " " - << _state.bias_a.transpose() << " " << V3D(_state.inv_expo_time, 0, 0).transpose() << endl; - - if (feats_undistort->empty() || (feats_undistort == nullptr)) - { - std::cout << "[ LIO ]: No point!!!" << std::endl; - return; - } - - double t0 = omp_get_wtime(); - - downSizeFilterSurf.setInputCloud(feats_undistort); - downSizeFilterSurf.filter(*feats_down_body); - - double t_down = omp_get_wtime(); - - feats_down_size = feats_down_body->points.size(); - voxelmap_manager->feats_down_body_ = feats_down_body; - transformLidar(_state.rot_end, _state.pos_end, feats_down_body, feats_down_world); - voxelmap_manager->feats_down_world_ = feats_down_world; - voxelmap_manager->feats_down_size_ = feats_down_size; - - if (!lidar_map_inited) - { - lidar_map_inited = true; - voxelmap_manager->BuildVoxelMap(); - } - - double t1 = omp_get_wtime(); - - voxelmap_manager->StateEstimation(state_propagat); - _state = voxelmap_manager->state_; - _pv_list = voxelmap_manager->pv_list_; - - double t2 = omp_get_wtime(); - - if (imu_prop_enable) - { - ekf_finish_once = true; - latest_ekf_state = _state; - latest_ekf_time = LidarMeasures.last_lio_update_time; - state_update_flg = true; - } - - if (pose_output_en) - { - static bool pos_opend = false; - static int ocount = 0; - std::ofstream outFile, evoFile; - if (!pos_opend) + if (fabs((LidarMeasures.last_lio_update_time - _first_lidar_time) - plot_time) < (frame_cnt / 2 * 0.1)) + { + vio_manager->plot_flag = true; + } + else { - evoFile.open(std::string(ROOT_DIR) + "Log/result/" + seq_name + ".txt", std::ios::out); - pos_opend = true; - if (!evoFile.is_open()) ROS_ERROR("open fail\n"); - } - else + vio_manager->plot_flag = false; + } + + vio_manager->processFrame(LidarMeasures.measures.back().img, _pv_list, voxelmap_manager->voxel_map_, LidarMeasures.last_lio_update_time - _first_lidar_time); + + if (imu_prop_enable) { - evoFile.open(std::string(ROOT_DIR) + "Log/result/" + seq_name + ".txt", std::ios::app); - if (!evoFile.is_open()) ROS_ERROR("open fail\n"); + ekf_finish_once = true; + latest_ekf_state = _state; + latest_ekf_time = LidarMeasures.last_lio_update_time; + state_update_flg = true; } - Eigen::Matrix4d outT; - Eigen::Quaterniond q(_state.rot_end); - evoFile << std::fixed; - evoFile << LidarMeasures.last_lio_update_time << " " << _state.pos_end[0] << " " << _state.pos_end[1] << " " << _state.pos_end[2] << " " - << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << std::endl; - } - - euler_cur = RotMtoEuler(_state.rot_end); - geoQuat = tf::createQuaternionMsgFromRollPitchYaw(euler_cur(0), euler_cur(1), euler_cur(2)); - publish_odometry(pubOdomAftMapped); - - double t3 = omp_get_wtime(); - - PointCloudXYZI::Ptr world_lidar(new PointCloudXYZI()); - transformLidar(_state.rot_end, _state.pos_end, feats_down_body, world_lidar); - for (size_t i = 0; i < world_lidar->points.size(); i++) - { - voxelmap_manager->pv_list_[i].point_w << world_lidar->points[i].x, world_lidar->points[i].y, world_lidar->points[i].z; - M3D point_crossmat = voxelmap_manager->cross_mat_list_[i]; - M3D var = voxelmap_manager->body_cov_list_[i]; - var = (_state.rot_end * extR) * var * (_state.rot_end * extR).transpose() + - (-point_crossmat) * _state.cov.block<3, 3>(0, 0) * (-point_crossmat).transpose() + _state.cov.block<3, 3>(3, 3); - voxelmap_manager->pv_list_[i].var = var; - } - voxelmap_manager->UpdateVoxelMap(voxelmap_manager->pv_list_); - std::cout << "[ LIO ] Update Voxel Map" << std::endl; - _pv_list = voxelmap_manager->pv_list_; - - double t4 = omp_get_wtime(); - - if(voxelmap_manager->config_setting_.map_sliding_en) - { - voxelmap_manager->mapSliding(); - } - - PointCloudXYZI::Ptr laserCloudFullRes(dense_map_en ? feats_undistort : feats_down_body); - int size = laserCloudFullRes->points.size(); - PointCloudXYZI::Ptr laserCloudWorld(new PointCloudXYZI(size, 1)); - - for (int i = 0; i < size; i++) - { - RGBpointBodyToWorld(&laserCloudFullRes->points[i], &laserCloudWorld->points[i]); - } - *pcl_w_wait_pub = *laserCloudWorld; - - if (!img_en) publish_frame_world(pubLaserCloudFullRes, vio_manager); - if (pub_effect_point_en) publish_effect_world(pubLaserCloudEffect, voxelmap_manager->ptpl_list_); - if (voxelmap_manager->config_setting_.is_pub_plane_map_) voxelmap_manager->pubVoxelMap(); - publish_path(pubPath); - publish_mavros(mavros_pose_publisher); - - frame_num++; - aver_time_consu = aver_time_consu * (frame_num - 1) / frame_num + (t4 - t0) / frame_num; - - // aver_time_icp = aver_time_icp * (frame_num - 1) / frame_num + (t2 - t1) / frame_num; - // aver_time_map_inre = aver_time_map_inre * (frame_num - 1) / frame_num + (t4 - t3) / frame_num; - // aver_time_solve = aver_time_solve * (frame_num - 1) / frame_num + (solve_time) / frame_num; - // aver_time_const_H_time = aver_time_const_H_time * (frame_num - 1) / frame_num + solve_const_H_time / frame_num; - // printf("[ mapping time ]: per scan: propagation %0.6f downsample: %0.6f match: %0.6f solve: %0.6f ICP: %0.6f map incre: %0.6f total: %0.6f \n" - // "[ mapping time ]: average: icp: %0.6f construct H: %0.6f, total: %0.6f \n", - // t_prop - t0, t1 - t_prop, match_time, solve_time, t3 - t1, t5 - t3, t5 - t0, aver_time_icp, aver_time_const_H_time, aver_time_consu); - - // printf("\033[1;36m[ LIO mapping time ]: current scan: icp: %0.6f secs, map incre: %0.6f secs, total: %0.6f secs.\033[0m\n" - // "\033[1;36m[ LIO mapping time ]: average: icp: %0.6f secs, map incre: %0.6f secs, total: %0.6f secs.\033[0m\n", - // t2 - t1, t4 - t3, t4 - t0, aver_time_icp, aver_time_map_inre, aver_time_consu); - printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n"); - printf("\033[1;34m| LIO Mapping Time |\033[0m\n"); - printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n"); - printf("\033[1;34m| %-29s | %-27s |\033[0m\n", "Algorithm Stage", "Time (secs)"); - printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n"); - printf("\033[1;36m| %-29s | %-27f |\033[0m\n", "DownSample", t_down - t0); - printf("\033[1;36m| %-29s | %-27f |\033[0m\n", "ICP", t2 - t1); - printf("\033[1;36m| %-29s | %-27f |\033[0m\n", "updateVoxelMap", t4 - t3); - printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n"); - printf("\033[1;36m| %-29s | %-27f |\033[0m\n", "Current Total Time", t4 - t0); - printf("\033[1;36m| %-29s | %-27f |\033[0m\n", "Average Total Time", aver_time_consu); - printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n"); - - euler_cur = RotMtoEuler(_state.rot_end); - fout_out << std::setw(20) << LidarMeasures.last_lio_update_time - _first_lidar_time << " " << euler_cur.transpose() * 57.3 << " " - << _state.pos_end.transpose() << " " << _state.vel_end.transpose() << " " << _state.bias_g.transpose() << " " - << _state.bias_a.transpose() << " " << V3D(_state.inv_expo_time, 0, 0).transpose() << " " << feats_undistort->points.size() << std::endl; + + // int size_sub_map = vio_manager->visual_sub_map_cur.size(); + // visual_sub_map->reserve(size_sub_map); + // for (int i = 0; i < size_sub_map; i++) + // { + // PointType temp_map; + // temp_map.x = vio_manager->visual_sub_map_cur[i]->pos_[0]; + // temp_map.y = vio_manager->visual_sub_map_cur[i]->pos_[1]; + // temp_map.z = vio_manager->visual_sub_map_cur[i]->pos_[2]; + // temp_map.intensity = 0.; + // visual_sub_map->push_back(temp_map); + // } + + publish_frame_world(pubLaserCloudFullRes, vio_manager); + publish_img_rgb(pubImage, vio_manager); + + euler_cur = RotMtoEuler(_state.rot_end); + fout_out << std::setw(20) << LidarMeasures.last_lio_update_time - _first_lidar_time << " " << euler_cur.transpose() * 57.3 << " " + << _state.pos_end.transpose() << " " << _state.vel_end.transpose() << " " << _state.bias_g.transpose() << " " + << _state.bias_a.transpose() << " " << V3D(_state.inv_expo_time, 0, 0).transpose() << " " << feats_undistort->points.size() << std::endl; } -void LIVMapper::savePCD() +void LIVMapper::handleLIO() { - if (pcd_save_en && (pcl_wait_save->points.size() > 0 || pcl_wait_save_intensity->points.size() > 0) && pcd_save_interval < 0) - { - std::string raw_points_dir = std::string(ROOT_DIR) + "Log/PCD/all_raw_points.pcd"; - std::string downsampled_points_dir = std::string(ROOT_DIR) + "Log/PCD/all_downsampled_points.pcd"; - pcl::PCDWriter pcd_writer; + euler_cur = RotMtoEuler(_state.rot_end); + fout_pre << setw(20) << LidarMeasures.last_lio_update_time - _first_lidar_time << " " << euler_cur.transpose() * 57.3 << " " + << _state.pos_end.transpose() << " " << _state.vel_end.transpose() << " " << _state.bias_g.transpose() << " " + << _state.bias_a.transpose() << " " << V3D(_state.inv_expo_time, 0, 0).transpose() << endl; - if (img_en) + if (feats_undistort->empty() || (feats_undistort == nullptr)) + { + std::cout << "[ LIO ]: No point!!!" << std::endl; + return; + } + + double t0 = omp_get_wtime(); + + downSizeFilterSurf.setInputCloud(feats_undistort); + downSizeFilterSurf.filter(*feats_down_body); + + double t_down = omp_get_wtime(); + + feats_down_size = feats_down_body->points.size(); + voxelmap_manager->feats_down_body_ = feats_down_body; + transformLidar(_state.rot_end, _state.pos_end, feats_down_body, feats_down_world); + voxelmap_manager->feats_down_world_ = feats_down_world; + voxelmap_manager->feats_down_size_ = feats_down_size; + + if (!lidar_map_inited) + { + lidar_map_inited = true; + voxelmap_manager->BuildVoxelMap(); + } + + double t1 = omp_get_wtime(); + + voxelmap_manager->StateEstimation(state_propagat); + _state = voxelmap_manager->state_; + _pv_list = voxelmap_manager->pv_list_; + + double t2 = omp_get_wtime(); + + if (imu_prop_enable) { - pcl::PointCloud::Ptr downsampled_cloud(new pcl::PointCloud); - pcl::VoxelGrid voxel_filter; - voxel_filter.setInputCloud(pcl_wait_save); - voxel_filter.setLeafSize(filter_size_pcd, filter_size_pcd, filter_size_pcd); - voxel_filter.filter(*downsampled_cloud); - - pcd_writer.writeBinary(raw_points_dir, *pcl_wait_save); // Save the raw point cloud data - std::cout << GREEN << "Raw point cloud data saved to: " << raw_points_dir - << " with point count: " << pcl_wait_save->points.size() << RESET << std::endl; - - pcd_writer.writeBinary(downsampled_points_dir, *downsampled_cloud); // Save the downsampled point cloud data - std::cout << GREEN << "Downsampled point cloud data saved to: " << downsampled_points_dir - << " with point count after filtering: " << downsampled_cloud->points.size() << RESET << std::endl; - - if(colmap_output_en) - { - fout_points << "# 3D point list with one line of data per point\n"; - fout_points << "# POINT_ID, X, Y, Z, R, G, B, ERROR\n"; - for (size_t i = 0; i < downsampled_cloud->size(); ++i) + ekf_finish_once = true; + latest_ekf_state = _state; + latest_ekf_time = LidarMeasures.last_lio_update_time; + state_update_flg = true; + } + + if (pose_output_en) + { + static bool pos_opend = false; + static int ocount = 0; + std::ofstream outFile, evoFile; + if (!pos_opend) + { + evoFile.open(std::string(ROOT_DIR) + "Log/result/" + seq_name + ".txt", std::ios::out); + pos_opend = true; + if (!evoFile.is_open()) + ROS_ERROR("open fail\n"); + } + else { - const auto& point = downsampled_cloud->points[i]; - fout_points << i << " " - << std::fixed << std::setprecision(6) - << point.x << " " << point.y << " " << point.z << " " - << static_cast(point.r) << " " - << static_cast(point.g) << " " - << static_cast(point.b) << " " - << 0 << std::endl; + evoFile.open(std::string(ROOT_DIR) + "Log/result/" + seq_name + ".txt", std::ios::app); + if (!evoFile.is_open()) + ROS_ERROR("open fail\n"); } - } + Eigen::Matrix4d outT; + Eigen::Quaterniond q(_state.rot_end); + evoFile << std::fixed; + evoFile << LidarMeasures.last_lio_update_time << " " << _state.pos_end[0] << " " << _state.pos_end[1] << " " << _state.pos_end[2] << " " + << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << std::endl; } - else - { - pcd_writer.writeBinary(raw_points_dir, *pcl_wait_save_intensity); - std::cout << GREEN << "Raw point cloud data saved to: " << raw_points_dir - << " with point count: " << pcl_wait_save_intensity->points.size() << RESET << std::endl; + + euler_cur = RotMtoEuler(_state.rot_end); + geoQuat = tf::createQuaternionMsgFromRollPitchYaw(euler_cur(0), euler_cur(1), euler_cur(2)); + publish_odometry(pubOdomAftMapped); + + double t3 = omp_get_wtime(); + + PointCloudXYZI::Ptr world_lidar(new PointCloudXYZI()); + transformLidar(_state.rot_end, _state.pos_end, feats_down_body, world_lidar); + for (size_t i = 0; i < world_lidar->points.size(); i++) + { + voxelmap_manager->pv_list_[i].point_w << world_lidar->points[i].x, world_lidar->points[i].y, world_lidar->points[i].z; + M3D point_crossmat = voxelmap_manager->cross_mat_list_[i]; + M3D var = voxelmap_manager->body_cov_list_[i]; + var = (_state.rot_end * extR) * var * (_state.rot_end * extR).transpose() + + (-point_crossmat) * _state.cov.block<3, 3>(0, 0) * (-point_crossmat).transpose() + _state.cov.block<3, 3>(3, 3); + voxelmap_manager->pv_list_[i].var = var; + } + voxelmap_manager->UpdateVoxelMap(voxelmap_manager->pv_list_); + std::cout << "[ LIO ] Update Voxel Map" << std::endl; + _pv_list = voxelmap_manager->pv_list_; + + double t4 = omp_get_wtime(); + + if (voxelmap_manager->config_setting_.map_sliding_en) + { + voxelmap_manager->mapSliding(); + } + + PointCloudXYZI::Ptr laserCloudFullRes(dense_map_en ? feats_undistort : feats_down_body); + int size = laserCloudFullRes->points.size(); + PointCloudXYZI::Ptr laserCloudWorld(new PointCloudXYZI(size, 1)); + + for (int i = 0; i < size; i++) + { + RGBpointBodyToWorld(&laserCloudFullRes->points[i], &laserCloudWorld->points[i]); } - } + *pcl_w_wait_pub = *laserCloudWorld; + + if (!img_en) + publish_frame_world(pubLaserCloudFullRes, vio_manager); + if (pub_effect_point_en) + publish_effect_world(pubLaserCloudEffect, voxelmap_manager->ptpl_list_); + if (voxelmap_manager->config_setting_.is_pub_plane_map_) + voxelmap_manager->pubVoxelMap(); + publish_path(pubPath); + publish_mavros(mavros_pose_publisher); + + frame_num++; + aver_time_consu = aver_time_consu * (frame_num - 1) / frame_num + (t4 - t0) / frame_num; + + // aver_time_icp = aver_time_icp * (frame_num - 1) / frame_num + (t2 - t1) / frame_num; + // aver_time_map_inre = aver_time_map_inre * (frame_num - 1) / frame_num + (t4 - t3) / frame_num; + // aver_time_solve = aver_time_solve * (frame_num - 1) / frame_num + (solve_time) / frame_num; + // aver_time_const_H_time = aver_time_const_H_time * (frame_num - 1) / frame_num + solve_const_H_time / frame_num; + // printf("[ mapping time ]: per scan: propagation %0.6f downsample: %0.6f match: %0.6f solve: %0.6f ICP: %0.6f map incre: %0.6f total: %0.6f \n" + // "[ mapping time ]: average: icp: %0.6f construct H: %0.6f, total: %0.6f \n", + // t_prop - t0, t1 - t_prop, match_time, solve_time, t3 - t1, t5 - t3, t5 - t0, aver_time_icp, aver_time_const_H_time, aver_time_consu); + + // printf("\033[1;36m[ LIO mapping time ]: current scan: icp: %0.6f secs, map incre: %0.6f secs, total: %0.6f secs.\033[0m\n" + // "\033[1;36m[ LIO mapping time ]: average: icp: %0.6f secs, map incre: %0.6f secs, total: %0.6f secs.\033[0m\n", + // t2 - t1, t4 - t3, t4 - t0, aver_time_icp, aver_time_map_inre, aver_time_consu); + printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n"); + printf("\033[1;34m| LIO Mapping Time |\033[0m\n"); + printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n"); + printf("\033[1;34m| %-29s | %-27s |\033[0m\n", "Algorithm Stage", "Time (secs)"); + printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n"); + printf("\033[1;36m| %-29s | %-27f |\033[0m\n", "DownSample", t_down - t0); + printf("\033[1;36m| %-29s | %-27f |\033[0m\n", "ICP", t2 - t1); + printf("\033[1;36m| %-29s | %-27f |\033[0m\n", "updateVoxelMap", t4 - t3); + printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n"); + printf("\033[1;36m| %-29s | %-27f |\033[0m\n", "Current Total Time", t4 - t0); + printf("\033[1;36m| %-29s | %-27f |\033[0m\n", "Average Total Time", aver_time_consu); + printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n"); + + euler_cur = RotMtoEuler(_state.rot_end); + fout_out << std::setw(20) << LidarMeasures.last_lio_update_time - _first_lidar_time << " " << euler_cur.transpose() * 57.3 << " " + << _state.pos_end.transpose() << " " << _state.vel_end.transpose() << " " << _state.bias_g.transpose() << " " + << _state.bias_a.transpose() << " " << V3D(_state.inv_expo_time, 0, 0).transpose() << " " << feats_undistort->points.size() << std::endl; } -void LIVMapper::run() +void LIVMapper::savePCD() { - ros::Rate rate(5000); - while (ros::ok()) - { - ros::spinOnce(); - if (!sync_packages(LidarMeasures)) + if (pcd_save_en && (pcl_wait_save->points.size() > 0 || pcl_wait_save_intensity->points.size() > 0) && pcd_save_interval < 0) { - rate.sleep(); - continue; + std::string raw_points_dir = std::string(ROOT_DIR) + "Log/PCD/all_raw_points.pcd"; + std::string downsampled_points_dir = std::string(ROOT_DIR) + "Log/PCD/all_downsampled_points.pcd"; + pcl::PCDWriter pcd_writer; + + if (img_en) + { + pcl::PointCloud::Ptr downsampled_cloud(new pcl::PointCloud); + pcl::VoxelGrid voxel_filter; + voxel_filter.setInputCloud(pcl_wait_save); + voxel_filter.setLeafSize(filter_size_pcd, filter_size_pcd, filter_size_pcd); + voxel_filter.filter(*downsampled_cloud); + + pcd_writer.writeBinary(raw_points_dir, *pcl_wait_save); // Save the raw point cloud data + std::cout << GREEN << "Raw point cloud data saved to: " << raw_points_dir + << " with point count: " << pcl_wait_save->points.size() << RESET << std::endl; + + pcd_writer.writeBinary(downsampled_points_dir, *downsampled_cloud); // Save the downsampled point cloud data + std::cout << GREEN << "Downsampled point cloud data saved to: " << downsampled_points_dir + << " with point count after filtering: " << downsampled_cloud->points.size() << RESET << std::endl; + + if (colmap_output_en) + { + fout_points << "# 3D point list with one line of data per point\n"; + fout_points << "# POINT_ID, X, Y, Z, R, G, B, ERROR\n"; + for (size_t i = 0; i < downsampled_cloud->size(); ++i) + { + const auto &point = downsampled_cloud->points[i]; + fout_points << i << " " + << std::fixed << std::setprecision(6) + << point.x << " " << point.y << " " << point.z << " " + << static_cast(point.r) << " " + << static_cast(point.g) << " " + << static_cast(point.b) << " " + << 0 << std::endl; + } + } + } + else + { + pcd_writer.writeBinary(raw_points_dir, *pcl_wait_save_intensity); + std::cout << GREEN << "Raw point cloud data saved to: " << raw_points_dir + << " with point count: " << pcl_wait_save_intensity->points.size() << RESET << std::endl; + } } - handleFirstFrame(); +} + +void LIVMapper::run() +{ + ros::Rate rate(5000); + while (ros::ok()) + { + ros::spinOnce(); + if (!sync_packages(LidarMeasures)) + { + rate.sleep(); + continue; + } + handleFirstFrame(); - processImu(); + processImu(); - // if (!p_imu->imu_time_init) continue; + // if (!p_imu->imu_time_init) continue; - stateEstimationAndMapping(); - } - savePCD(); + stateEstimationAndMapping(); + } + savePCD(); } void LIVMapper::prop_imu_once(StatesGroup &imu_prop_state, const double dt, V3D acc_avr, V3D angvel_avr) { - double mean_acc_norm = p_imu->IMU_mean_acc_norm; - acc_avr = acc_avr * G_m_s2 / mean_acc_norm - imu_prop_state.bias_a; - angvel_avr -= imu_prop_state.bias_g; + double mean_acc_norm = p_imu->IMU_mean_acc_norm; + acc_avr = acc_avr * G_m_s2 / mean_acc_norm - imu_prop_state.bias_a; + angvel_avr -= imu_prop_state.bias_g; - M3D Exp_f = Exp(angvel_avr, dt); - /* propogation of IMU attitude */ - imu_prop_state.rot_end = imu_prop_state.rot_end * Exp_f; + M3D Exp_f = Exp(angvel_avr, dt); + /* propogation of IMU attitude */ + imu_prop_state.rot_end = imu_prop_state.rot_end * Exp_f; - /* Specific acceleration (global frame) of IMU */ - V3D acc_imu = imu_prop_state.rot_end * acc_avr + V3D(imu_prop_state.gravity[0], imu_prop_state.gravity[1], imu_prop_state.gravity[2]); + /* Specific acceleration (global frame) of IMU */ + V3D acc_imu = imu_prop_state.rot_end * acc_avr + V3D(imu_prop_state.gravity[0], imu_prop_state.gravity[1], imu_prop_state.gravity[2]); - /* propogation of IMU */ - imu_prop_state.pos_end = imu_prop_state.pos_end + imu_prop_state.vel_end * dt + 0.5 * acc_imu * dt * dt; + /* propogation of IMU */ + imu_prop_state.pos_end = imu_prop_state.pos_end + imu_prop_state.vel_end * dt + 0.5 * acc_imu * dt * dt; - /* velocity of IMU */ - imu_prop_state.vel_end = imu_prop_state.vel_end + acc_imu * dt; + /* velocity of IMU */ + imu_prop_state.vel_end = imu_prop_state.vel_end + acc_imu * dt; } void LIVMapper::imu_prop_callback(const ros::TimerEvent &e) { - if (p_imu->imu_need_init || !new_imu || !ekf_finish_once) { return; } - mtx_buffer_imu_prop.lock(); - new_imu = false; // 控制propagate频率和IMU频率一致 - if (imu_prop_enable && !prop_imu_buffer.empty()) - { - static double last_t_from_lidar_end_time = 0; - if (state_update_flg) + if (p_imu->imu_need_init || !new_imu || !ekf_finish_once) { - imu_propagate = latest_ekf_state; - // drop all useless imu pkg - while ((!prop_imu_buffer.empty() && prop_imu_buffer.front().header.stamp.toSec() < latest_ekf_time)) - { - prop_imu_buffer.pop_front(); - } - last_t_from_lidar_end_time = 0; - for (int i = 0; i < prop_imu_buffer.size(); i++) - { - double t_from_lidar_end_time = prop_imu_buffer[i].header.stamp.toSec() - latest_ekf_time; - double dt = t_from_lidar_end_time - last_t_from_lidar_end_time; - // cout << "prop dt" << dt << ", " << t_from_lidar_end_time << ", " << last_t_from_lidar_end_time << endl; - V3D acc_imu(prop_imu_buffer[i].linear_acceleration.x, prop_imu_buffer[i].linear_acceleration.y, prop_imu_buffer[i].linear_acceleration.z); - V3D omg_imu(prop_imu_buffer[i].angular_velocity.x, prop_imu_buffer[i].angular_velocity.y, prop_imu_buffer[i].angular_velocity.z); - prop_imu_once(imu_propagate, dt, acc_imu, omg_imu); - last_t_from_lidar_end_time = t_from_lidar_end_time; - } - state_update_flg = false; + return; } - else + mtx_buffer_imu_prop.lock(); + new_imu = false; // 控制propagate频率和IMU频率一致 + if (imu_prop_enable && !prop_imu_buffer.empty()) { - V3D acc_imu(newest_imu.linear_acceleration.x, newest_imu.linear_acceleration.y, newest_imu.linear_acceleration.z); - V3D omg_imu(newest_imu.angular_velocity.x, newest_imu.angular_velocity.y, newest_imu.angular_velocity.z); - double t_from_lidar_end_time = newest_imu.header.stamp.toSec() - latest_ekf_time; - double dt = t_from_lidar_end_time - last_t_from_lidar_end_time; - prop_imu_once(imu_propagate, dt, acc_imu, omg_imu); - last_t_from_lidar_end_time = t_from_lidar_end_time; - } + static double last_t_from_lidar_end_time = 0; + if (state_update_flg) + { + imu_propagate = latest_ekf_state; + // drop all useless imu pkg + while ((!prop_imu_buffer.empty() && prop_imu_buffer.front().header.stamp.toSec() < latest_ekf_time)) + { + prop_imu_buffer.pop_front(); + } + last_t_from_lidar_end_time = 0; + for (int i = 0; i < prop_imu_buffer.size(); i++) + { + double t_from_lidar_end_time = prop_imu_buffer[i].header.stamp.toSec() - latest_ekf_time; + double dt = t_from_lidar_end_time - last_t_from_lidar_end_time; + // cout << "prop dt" << dt << ", " << t_from_lidar_end_time << ", " << last_t_from_lidar_end_time << endl; + V3D acc_imu(prop_imu_buffer[i].linear_acceleration.x, prop_imu_buffer[i].linear_acceleration.y, prop_imu_buffer[i].linear_acceleration.z); + V3D omg_imu(prop_imu_buffer[i].angular_velocity.x, prop_imu_buffer[i].angular_velocity.y, prop_imu_buffer[i].angular_velocity.z); + prop_imu_once(imu_propagate, dt, acc_imu, omg_imu); + last_t_from_lidar_end_time = t_from_lidar_end_time; + } + state_update_flg = false; + } + else + { + V3D acc_imu(newest_imu.linear_acceleration.x, newest_imu.linear_acceleration.y, newest_imu.linear_acceleration.z); + V3D omg_imu(newest_imu.angular_velocity.x, newest_imu.angular_velocity.y, newest_imu.angular_velocity.z); + double t_from_lidar_end_time = newest_imu.header.stamp.toSec() - latest_ekf_time; + double dt = t_from_lidar_end_time - last_t_from_lidar_end_time; + prop_imu_once(imu_propagate, dt, acc_imu, omg_imu); + last_t_from_lidar_end_time = t_from_lidar_end_time; + } - V3D posi, vel_i; - Eigen::Quaterniond q; - posi = imu_propagate.pos_end; - vel_i = imu_propagate.vel_end; - q = Eigen::Quaterniond(imu_propagate.rot_end); - imu_prop_odom.header.frame_id = "world"; - imu_prop_odom.header.stamp = newest_imu.header.stamp; - imu_prop_odom.pose.pose.position.x = posi.x(); - imu_prop_odom.pose.pose.position.y = posi.y(); - imu_prop_odom.pose.pose.position.z = posi.z(); - imu_prop_odom.pose.pose.orientation.w = q.w(); - imu_prop_odom.pose.pose.orientation.x = q.x(); - imu_prop_odom.pose.pose.orientation.y = q.y(); - imu_prop_odom.pose.pose.orientation.z = q.z(); - imu_prop_odom.twist.twist.linear.x = vel_i.x(); - imu_prop_odom.twist.twist.linear.y = vel_i.y(); - imu_prop_odom.twist.twist.linear.z = vel_i.z(); - pubImuPropOdom.publish(imu_prop_odom); - } - mtx_buffer_imu_prop.unlock(); + V3D posi, vel_i; + Eigen::Quaterniond q; + posi = imu_propagate.pos_end; + vel_i = imu_propagate.vel_end; + q = Eigen::Quaterniond(imu_propagate.rot_end); + imu_prop_odom.header.frame_id = "world"; + imu_prop_odom.header.stamp = newest_imu.header.stamp; + imu_prop_odom.pose.pose.position.x = posi.x(); + imu_prop_odom.pose.pose.position.y = posi.y(); + imu_prop_odom.pose.pose.position.z = posi.z(); + imu_prop_odom.pose.pose.orientation.w = q.w(); + imu_prop_odom.pose.pose.orientation.x = q.x(); + imu_prop_odom.pose.pose.orientation.y = q.y(); + imu_prop_odom.pose.pose.orientation.z = q.z(); + imu_prop_odom.twist.twist.linear.x = vel_i.x(); + imu_prop_odom.twist.twist.linear.y = vel_i.y(); + imu_prop_odom.twist.twist.linear.z = vel_i.z(); + pubImuPropOdom.publish(imu_prop_odom); + } + mtx_buffer_imu_prop.unlock(); } void LIVMapper::transformLidar(const Eigen::Matrix3d rot, const Eigen::Vector3d t, const PointCloudXYZI::Ptr &input_cloud, PointCloudXYZI::Ptr &trans_cloud) { - PointCloudXYZI().swap(*trans_cloud); - trans_cloud->reserve(input_cloud->size()); - for (size_t i = 0; i < input_cloud->size(); i++) - { - pcl::PointXYZINormal p_c = input_cloud->points[i]; - Eigen::Vector3d p(p_c.x, p_c.y, p_c.z); - p = (rot * (extR * p + extT) + t); - PointType pi; - pi.x = p(0); - pi.y = p(1); - pi.z = p(2); - pi.intensity = p_c.intensity; - trans_cloud->points.push_back(pi); - } + PointCloudXYZI().swap(*trans_cloud); + trans_cloud->reserve(input_cloud->size()); + for (size_t i = 0; i < input_cloud->size(); i++) + { + pcl::PointXYZINormal p_c = input_cloud->points[i]; + Eigen::Vector3d p(p_c.x, p_c.y, p_c.z); + p = (rot * (extR * p + extT) + t); + PointType pi; + pi.x = p(0); + pi.y = p(1); + pi.z = p(2); + pi.intensity = p_c.intensity; + trans_cloud->points.push_back(pi); + } } void LIVMapper::pointBodyToWorld(const PointType &pi, PointType &po) { - V3D p_body(pi.x, pi.y, pi.z); - V3D p_global(_state.rot_end * (extR * p_body + extT) + _state.pos_end); - po.x = p_global(0); - po.y = p_global(1); - po.z = p_global(2); - po.intensity = pi.intensity; + V3D p_body(pi.x, pi.y, pi.z); + V3D p_global(_state.rot_end * (extR * p_body + extT) + _state.pos_end); + po.x = p_global(0); + po.y = p_global(1); + po.z = p_global(2); + po.intensity = pi.intensity; } -template void LIVMapper::pointBodyToWorld(const Matrix &pi, Matrix &po) +template +void LIVMapper::pointBodyToWorld(const Matrix &pi, Matrix &po) { - V3D p_body(pi[0], pi[1], pi[2]); - V3D p_global(_state.rot_end * (extR * p_body + extT) + _state.pos_end); - po[0] = p_global(0); - po[1] = p_global(1); - po[2] = p_global(2); + V3D p_body(pi[0], pi[1], pi[2]); + V3D p_global(_state.rot_end * (extR * p_body + extT) + _state.pos_end); + po[0] = p_global(0); + po[1] = p_global(1); + po[2] = p_global(2); } -template Matrix LIVMapper::pointBodyToWorld(const Matrix &pi) +template +Matrix LIVMapper::pointBodyToWorld(const Matrix &pi) { - V3D p(pi[0], pi[1], pi[2]); - p = (_state.rot_end * (extR * p + extT) + _state.pos_end); - Matrix po(p[0], p[1], p[2]); - return po; + V3D p(pi[0], pi[1], pi[2]); + p = (_state.rot_end * (extR * p + extT) + _state.pos_end); + Matrix po(p[0], p[1], p[2]); + return po; } void LIVMapper::RGBpointBodyToWorld(PointType const *const pi, PointType *const po) { - V3D p_body(pi->x, pi->y, pi->z); - V3D p_global(_state.rot_end * (extR * p_body + extT) + _state.pos_end); - po->x = p_global(0); - po->y = p_global(1); - po->z = p_global(2); - po->intensity = pi->intensity; + V3D p_body(pi->x, pi->y, pi->z); + V3D p_global(_state.rot_end * (extR * p_body + extT) + _state.pos_end); + po->x = p_global(0); + po->y = p_global(1); + po->z = p_global(2); + po->intensity = pi->intensity; } void LIVMapper::standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg) { - if (!lidar_en) return; - mtx_buffer.lock(); - - double cur_head_time = msg->header.stamp.toSec() + lidar_time_offset; - // cout<<"got feature"<header.stamp.toSec()); - PointCloudXYZI::Ptr ptr(new PointCloudXYZI()); - p_pre->process(msg, ptr); - lid_raw_data_buffer.push_back(ptr); - lid_header_time_buffer.push_back(cur_head_time); - last_timestamp_lidar = cur_head_time; - - mtx_buffer.unlock(); - sig_buffer.notify_all(); + if (!lidar_en) + return; + mtx_buffer.lock(); + + double cur_head_time = msg->header.stamp.toSec() + lidar_time_offset; + // cout<<"got feature"<header.stamp.toSec()); + PointCloudXYZI::Ptr ptr(new PointCloudXYZI()); + p_pre->process(msg, ptr); + lid_raw_data_buffer.push_back(ptr); + lid_header_time_buffer.push_back(cur_head_time); + last_timestamp_lidar = cur_head_time; + + mtx_buffer.unlock(); + sig_buffer.notify_all(); } void LIVMapper::livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg_in) { - if (!lidar_en) return; - mtx_buffer.lock(); - livox_ros_driver::CustomMsg::Ptr msg(new livox_ros_driver::CustomMsg(*msg_in)); - // if ((abs(msg->header.stamp.toSec() - last_timestamp_lidar) > 0.2 && last_timestamp_lidar > 0) || sync_jump_flag) - // { - // ROS_WARN("lidar jumps %.3f\n", msg->header.stamp.toSec() - last_timestamp_lidar); - // sync_jump_flag = true; - // msg->header.stamp = ros::Time().fromSec(last_timestamp_lidar + 0.1); - // } - if (abs(last_timestamp_imu - msg->header.stamp.toSec()) > 1.0 && !imu_buffer.empty()) - { - double timediff_imu_wrt_lidar = last_timestamp_imu - msg->header.stamp.toSec(); - printf("\033[95mSelf sync IMU and LiDAR, HARD time lag is %.10lf \n\033[0m", timediff_imu_wrt_lidar - 0.100); - // imu_time_offset = timediff_imu_wrt_lidar; - } - - double cur_head_time = msg->header.stamp.toSec(); - ROS_INFO("Get LiDAR, its header time: %.6f", cur_head_time); - if (cur_head_time < last_timestamp_lidar) - { - ROS_ERROR("lidar loop back, clear buffer"); - lid_raw_data_buffer.clear(); - } - // ROS_INFO("get point cloud at time: %.6f", msg->header.stamp.toSec()); - PointCloudXYZI::Ptr ptr(new PointCloudXYZI()); - p_pre->process(msg, ptr); - - if (!ptr || ptr->empty()) { - ROS_ERROR("Received an empty point cloud"); - mtx_buffer.unlock(); - return; - } + if (!lidar_en) + return; + mtx_buffer.lock(); + livox_ros_driver::CustomMsg::Ptr msg(new livox_ros_driver::CustomMsg(*msg_in)); + // if ((abs(msg->header.stamp.toSec() - last_timestamp_lidar) > 0.2 && last_timestamp_lidar > 0) || sync_jump_flag) + // { + // ROS_WARN("lidar jumps %.3f\n", msg->header.stamp.toSec() - last_timestamp_lidar); + // sync_jump_flag = true; + // msg->header.stamp = ros::Time().fromSec(last_timestamp_lidar + 0.1); + // } + if (abs(last_timestamp_imu - msg->header.stamp.toSec()) > 1.0 && !imu_buffer.empty()) + { + double timediff_imu_wrt_lidar = last_timestamp_imu - msg->header.stamp.toSec(); + printf("\033[95mSelf sync IMU and LiDAR, HARD time lag is %.10lf \n\033[0m", timediff_imu_wrt_lidar - 0.100); + // imu_time_offset = timediff_imu_wrt_lidar; + } + + double cur_head_time = msg->header.stamp.toSec(); + ROS_INFO("Get LiDAR, its header time: %.6f", cur_head_time); + if (cur_head_time < last_timestamp_lidar) + { + ROS_ERROR("lidar loop back, clear buffer"); + lid_raw_data_buffer.clear(); + } + // ROS_INFO("get point cloud at time: %.6f", msg->header.stamp.toSec()); + PointCloudXYZI::Ptr ptr(new PointCloudXYZI()); + p_pre->process(msg, ptr); - lid_raw_data_buffer.push_back(ptr); - lid_header_time_buffer.push_back(cur_head_time); - last_timestamp_lidar = cur_head_time; + if (!ptr || ptr->empty()) + { + ROS_ERROR("Received an empty point cloud"); + mtx_buffer.unlock(); + return; + } - mtx_buffer.unlock(); - sig_buffer.notify_all(); + lid_raw_data_buffer.push_back(ptr); + lid_header_time_buffer.push_back(cur_head_time); + last_timestamp_lidar = cur_head_time; + + mtx_buffer.unlock(); + sig_buffer.notify_all(); } void LIVMapper::imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in) { - if (!imu_en) return; + if (!imu_en) + return; + + if (last_timestamp_lidar < 0.0) + return; + // ROS_INFO("get imu at time: %.6f", msg_in->header.stamp.toSec()); + sensor_msgs::Imu::Ptr msg(new sensor_msgs::Imu(*msg_in)); + msg->header.stamp = ros::Time().fromSec(msg->header.stamp.toSec() - imu_time_offset); + double timestamp = msg->header.stamp.toSec(); + + if (fabs(last_timestamp_lidar - timestamp) > 0.5 && (!ros_driver_fix_en)) + { + ROS_WARN("IMU and LiDAR not synced! delta time: %lf .\n", last_timestamp_lidar - timestamp); + } - if (last_timestamp_lidar < 0.0) return; - // ROS_INFO("get imu at time: %.6f", msg_in->header.stamp.toSec()); - sensor_msgs::Imu::Ptr msg(new sensor_msgs::Imu(*msg_in)); - msg->header.stamp = ros::Time().fromSec(msg->header.stamp.toSec() - imu_time_offset); - double timestamp = msg->header.stamp.toSec(); + if (ros_driver_fix_en) + timestamp += std::round(last_timestamp_lidar - timestamp); + msg->header.stamp = ros::Time().fromSec(timestamp); - if (fabs(last_timestamp_lidar - timestamp) > 0.5 && (!ros_driver_fix_en)) - { - ROS_WARN("IMU and LiDAR not synced! delta time: %lf .\n", last_timestamp_lidar - timestamp); - } + mtx_buffer.lock(); + + if (last_timestamp_imu > 0.0 && timestamp < last_timestamp_imu) + { + mtx_buffer.unlock(); + sig_buffer.notify_all(); + ROS_ERROR("imu loop back, offset: %lf \n", last_timestamp_imu - timestamp); + return; + } - if (ros_driver_fix_en) timestamp += std::round(last_timestamp_lidar - timestamp); - msg->header.stamp = ros::Time().fromSec(timestamp); + // if (last_timestamp_imu > 0.0 && timestamp > last_timestamp_imu + 0.2) + // { - mtx_buffer.lock(); + // ROS_WARN("imu time stamp Jumps %0.4lf seconds \n", timestamp - last_timestamp_imu); + // mtx_buffer.unlock(); + // sig_buffer.notify_all(); + // return; + // } - if (last_timestamp_imu > 0.0 && timestamp < last_timestamp_imu) - { + last_timestamp_imu = timestamp; + + imu_buffer.push_back(msg); + // cout<<"got imu: "<imu_need_init) + { + prop_imu_buffer.push_back(*msg); + } + newest_imu = *msg; + new_imu = true; + mtx_buffer_imu_prop.unlock(); + } sig_buffer.notify_all(); - ROS_ERROR("imu loop back, offset: %lf \n", last_timestamp_imu - timestamp); - return; - } - - // if (last_timestamp_imu > 0.0 && timestamp > last_timestamp_imu + 0.2) - // { - - // ROS_WARN("imu time stamp Jumps %0.4lf seconds \n", timestamp - last_timestamp_imu); - // mtx_buffer.unlock(); - // sig_buffer.notify_all(); - // return; - // } - - last_timestamp_imu = timestamp; - - imu_buffer.push_back(msg); - // cout<<"got imu: "<imu_need_init) { prop_imu_buffer.push_back(*msg); } - newest_imu = *msg; - new_imu = true; - mtx_buffer_imu_prop.unlock(); - } - sig_buffer.notify_all(); } cv::Mat LIVMapper::getImageFromMsg(const sensor_msgs::ImageConstPtr &img_msg) { - cv::Mat img; - img = cv_bridge::toCvCopy(img_msg, "bgr8")->image; - return img; + cv::Mat img; + img = cv_bridge::toCvCopy(img_msg, "bgr8")->image; + return img; } +cv::Mat LIVMapper::getImageFromCompressedMsg(const sensor_msgs::CompressedImageConstPtr &img_msg) +{ + cv::Mat img; + try + { + cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::BGR8); + img = cv_ptr->image; + } + catch (cv_bridge::Exception &e) + { + ROS_ERROR("cv_bridge exception: %s", e.what()); + } + return img; +} void LIVMapper::img_cbk(const sensor_msgs::ImageConstPtr &msg_in) { - if (!img_en) return; - sensor_msgs::Image::Ptr msg(new sensor_msgs::Image(*msg_in)); - // if ((abs(msg->header.stamp.toSec() - last_timestamp_img) > 0.2 && last_timestamp_img > 0) || sync_jump_flag) - // { - // ROS_WARN("img jumps %.3f\n", msg->header.stamp.toSec() - last_timestamp_img); - // sync_jump_flag = true; - // msg->header.stamp = ros::Time().fromSec(last_timestamp_img + 0.1); - // } - - // Hiliti2022 40Hz - if (hilti_en) - { - static int frame_counter = 0; - if (++frame_counter % 4 != 0) return; - } - // double msg_header_time = msg->header.stamp.toSec(); - double msg_header_time = msg->header.stamp.toSec() + img_time_offset; - if (abs(msg_header_time - last_timestamp_img) < 0.001) return; - ROS_INFO("Get image, its header time: %.6f", msg_header_time); - if (last_timestamp_lidar < 0) return; - - if (msg_header_time < last_timestamp_img) - { - ROS_ERROR("image loop back. \n"); - return; - } - - mtx_buffer.lock(); - - double img_time_correct = msg_header_time; // last_timestamp_lidar + 0.105; - - if (img_time_correct - last_timestamp_img < 0.02) - { - ROS_WARN("Image need Jumps: %.6f", img_time_correct); - mtx_buffer.unlock(); - sig_buffer.notify_all(); - return; - } + if (!img_en) + return; + sensor_msgs::Image::Ptr msg(new sensor_msgs::Image(*msg_in)); + // if ((abs(msg->header.stamp.toSec() - last_timestamp_img) > 0.2 && last_timestamp_img > 0) || sync_jump_flag) + // { + // ROS_WARN("img jumps %.3f\n", msg->header.stamp.toSec() - last_timestamp_img); + // sync_jump_flag = true; + // msg->header.stamp = ros::Time().fromSec(last_timestamp_img + 0.1); + // } + + // Hiliti2022 40Hz + if (hilti_en) + { + static int frame_counter = 0; + if (++frame_counter % 4 != 0) + return; + } + // double msg_header_time = msg->header.stamp.toSec(); + double msg_header_time = msg->header.stamp.toSec() + img_time_offset; + if (abs(msg_header_time - last_timestamp_img) < 0.001) + return; + ROS_INFO("Get image, its header time: %.6f", msg_header_time); + if (last_timestamp_lidar < 0) + return; + + if (msg_header_time < last_timestamp_img) + { + ROS_ERROR("image loop back. \n"); + return; + } + + mtx_buffer.lock(); + + double img_time_correct = msg_header_time; // last_timestamp_lidar + 0.105; - cv::Mat img_cur = getImageFromMsg(msg); - img_buffer.push_back(img_cur); - img_time_buffer.push_back(img_time_correct); + if (img_time_correct - last_timestamp_img < 0.02) + { + ROS_WARN("Image need Jumps: %.6f", img_time_correct); + mtx_buffer.unlock(); + sig_buffer.notify_all(); + return; + } - // ROS_INFO("Correct Image time: %.6f", img_time_correct); + cv::Mat img_cur = getImageFromMsg(msg); + img_buffer.push_back(img_cur); + img_time_buffer.push_back(img_time_correct); - last_timestamp_img = img_time_correct; - // cv::imshow("img", img); - // cv::waitKey(1); - // cout<<"last_timestamp_img:::"<header.stamp.toSec() - last_timestamp_img) > 0.2 && last_timestamp_img > 0) || sync_jump_flag) + // { + // ROS_WARN("img jumps %.3f\n", msg->header.stamp.toSec() - last_timestamp_img); + // sync_jump_flag = true; + // msg->header.stamp = ros::Time().fromSec(last_timestamp_img + 0.1); + // } + + // Hiliti2022 40Hz + if (hilti_en) { - // If not push the lidar into measurement data buffer - meas.lidar = lid_raw_data_buffer.front(); // push the first lidar topic - if (meas.lidar->points.size() <= 1) return false; - - meas.lidar_frame_beg_time = lid_header_time_buffer.front(); // generate lidar_frame_beg_time - meas.lidar_frame_end_time = meas.lidar_frame_beg_time + meas.lidar->points.back().curvature / double(1000); // calc lidar scan end time - meas.pcl_proc_cur = meas.lidar; - lidar_pushed = true; // flag + static int frame_counter = 0; + if (++frame_counter % 4 != 0) + return; } - - if (imu_en && last_timestamp_imu < meas.lidar_frame_end_time) - { // waiting imu message needs to be - // larger than _lidar_frame_end_time, - // make sure complete propagate. - // ROS_ERROR("out sync"); - return false; + // double msg_header_time = msg->header.stamp.toSec(); + double msg_header_time = msg->header.stamp.toSec() + img_time_offset; + if (abs(msg_header_time - last_timestamp_img) < 0.001) + return; + ROS_INFO("Get image, its header time: %.6f", msg_header_time); + if (last_timestamp_lidar < 0) + return; + + if (msg_header_time < last_timestamp_img) + { + ROS_ERROR("image loop back. \n"); + return; } - struct MeasureGroup m; // standard method to keep imu message. - - m.imu.clear(); - m.lio_time = meas.lidar_frame_end_time; mtx_buffer.lock(); - while (!imu_buffer.empty()) + + double img_time_correct = msg_header_time; // last_timestamp_lidar + 0.105; + + if (img_time_correct - last_timestamp_img < 0.02) { - if (imu_buffer.front()->header.stamp.toSec() > meas.lidar_frame_end_time) break; - m.imu.push_back(imu_buffer.front()); - imu_buffer.pop_front(); + ROS_WARN("Image need Jumps: %.6f", img_time_correct); + mtx_buffer.unlock(); + sig_buffer.notify_all(); + return; } - lid_raw_data_buffer.pop_front(); - lid_header_time_buffer.pop_front(); + + cv::Mat img_cur = getImageFromCompressedMsg(msg); + img_buffer.push_back(img_cur); + img_time_buffer.push_back(img_time_correct); + + // ROS_INFO("Correct Image time: %.6f", img_time_correct); + + last_timestamp_img = img_time_correct; + // cv::imshow("img", img); + // cv::waitKey(1); + // cout<<"last_timestamp_img:::"<header.stamp.toSec() - last_timestamp_img) > 0.2 && last_timestamp_img > 0) || sync_jump_flag) + // // { + // // ROS_WARN("img jumps %.3f\n", msg->header.stamp.toSec() - last_timestamp_img); + // // sync_jump_flag = true; + // // msg->header.stamp = ros::Time().fromSec(last_timestamp_img + 0.1); + // // } + + // // Hiliti2022 40Hz + // if (hilti_en) + // { + // static int frame_counter = 0; + // if (++frame_counter % 4 != 0) + // return; + // } + // // double msg_header_time = msg->header.stamp.toSec(); + // double msg_header_time = msg->header.stamp.toSec() + img_time_offset; + // if (abs(msg_header_time - last_timestamp_img) < 0.001) + // return; + // ROS_INFO("Get image, its header time: %.6f", msg_header_time); + // if (last_timestamp_lidar < 0) + // return; + + // if (msg_header_time < last_timestamp_img) + // { + // ROS_ERROR("image loop back. \n"); + // return; + // } + + // mtx_buffer.lock(); + + // double img_time_correct = msg_header_time; // last_timestamp_lidar + 0.105; + + // if (img_time_correct - last_timestamp_img < 0.02) + // { + // ROS_WARN("Image need Jumps: %.6f", img_time_correct); + // mtx_buffer.unlock(); + // sig_buffer.notify_all(); + // return; + // } + + // cv::Mat img_cur = getImageFromMsg(msg); + // img_buffer.push_back(img_cur); + // img_time_buffer.push_back(img_time_correct); + + // // ROS_INFO("Correct Image time: %.6f", img_time_correct); + + // last_timestamp_img = img_time_correct; + // // cv::imshow("img", img); + // // cv::waitKey(1); + // // cout<<"last_timestamp_img:::"<header.stamp.toSec() - last_timestamp_img) > 0.2 && last_timestamp_img > 0) || sync_jump_flag) + // { + // ROS_WARN("img jumps %.3f\n", msg->header.stamp.toSec() - last_timestamp_img); + // sync_jump_flag = true; + // msg->header.stamp = ros::Time().fromSec(last_timestamp_img + 0.1); + // } + + // Hiliti2022 40Hz + if (hilti_en) + { + static int frame_counter = 0; + if (++frame_counter % 4 != 0) + return; + } + // double msg_header_time = msg->header.stamp.toSec(); + double msg_header_time = img_ptr->header.stamp.toSec() + img_time_offset; + if (abs(msg_header_time - last_timestamp_img) < 0.001) + return; + ROS_INFO("Get image, its header time: %.6f", msg_header_time); + if (last_timestamp_lidar < 0) + return; + + if (msg_header_time < last_timestamp_img) + { + ROS_ERROR("image loop back. \n"); + return; + } + + mtx_buffer.lock(); + + double img_time_correct = msg_header_time; // last_timestamp_lidar + 0.105; + + if (img_time_correct - last_timestamp_img < 0.02) + { + ROS_WARN("Image need Jumps: %.6f", img_time_correct); + mtx_buffer.unlock(); + sig_buffer.notify_all(); + return; + } + + cv::Mat img_cur = getImageFromCompressedMsg(img_ptr); + cv::Mat mask_cur = getImageFromMsg(mask_ptr); + img_buffer.push_back(img_cur); + mask_buffer.push_back(mask_cur); + img_time_buffer.push_back(img_time_correct); + + // ROS_INFO("Correct Image time: %.6f", img_time_correct); + + last_timestamp_img = img_time_correct; + // cv::imshow("img", img); + // cv::waitKey(1); + // cout<<"last_timestamp_img:::"<points.back().curvature / double(1000); - double imu_newest_time = imu_buffer.back()->header.stamp.toSec(); - - if (img_capture_time < meas.last_lio_update_time + 0.00001) - { - img_buffer.pop_front(); - img_time_buffer.pop_front(); - ROS_ERROR("[ Data Cut ] Throw one image frame! \n"); - return false; - } + if (meas.last_lio_update_time < 0.0) + meas.last_lio_update_time = lid_header_time_buffer.front(); + if (!lidar_pushed) + { + // If not push the lidar into measurement data buffer + meas.lidar = lid_raw_data_buffer.front(); // push the first lidar topic + if (meas.lidar->points.size() <= 1) + return false; + + meas.lidar_frame_beg_time = lid_header_time_buffer.front(); // generate lidar_frame_beg_time + meas.lidar_frame_end_time = meas.lidar_frame_beg_time + meas.lidar->points.back().curvature / double(1000); // calc lidar scan end time + meas.pcl_proc_cur = meas.lidar; + lidar_pushed = true; // flag + } - if (img_capture_time > lid_newest_time || img_capture_time > imu_newest_time) - { - // ROS_ERROR("lost first camera frame"); - // printf("img_capture_time, lid_newest_time, imu_newest_time: %lf , %lf - // , %lf \n", img_capture_time, lid_newest_time, imu_newest_time); - return false; - } - - struct MeasureGroup m; - - // printf("[ Data Cut ] LIO \n"); - // printf("[ Data Cut ] img_capture_time: %lf \n", img_capture_time); - m.imu.clear(); - m.lio_time = img_capture_time; - mtx_buffer.lock(); - while (!imu_buffer.empty()) - { - if (imu_buffer.front()->header.stamp.toSec() > m.lio_time) break; - - if (imu_buffer.front()->header.stamp.toSec() > meas.last_lio_update_time) m.imu.push_back(imu_buffer.front()); - - imu_buffer.pop_front(); - // printf("[ Data Cut ] imu time: %lf \n", - // imu_buffer.front()->header.stamp.toSec()); - } - mtx_buffer.unlock(); - sig_buffer.notify_all(); - - *(meas.pcl_proc_cur) = *(meas.pcl_proc_next); - PointCloudXYZI().swap(*meas.pcl_proc_next); - - int lid_frame_num = lid_raw_data_buffer.size(); - int max_size = meas.pcl_proc_cur->size() + 24000 * lid_frame_num; - meas.pcl_proc_cur->reserve(max_size); - meas.pcl_proc_next->reserve(max_size); - // deque lidar_buffer_tmp; - - while (!lid_raw_data_buffer.empty()) - { - if (lid_header_time_buffer.front() > img_capture_time) break; - auto pcl(lid_raw_data_buffer.front()->points); - double frame_header_time(lid_header_time_buffer.front()); - float max_offs_time_ms = (m.lio_time - frame_header_time) * 1000.0f; - - for (int i = 0; i < pcl.size(); i++) + if (imu_en && last_timestamp_imu < meas.lidar_frame_end_time) + { // waiting imu message needs to be + // larger than _lidar_frame_end_time, + // make sure complete propagate. + // ROS_ERROR("out sync"); + return false; + } + + struct MeasureGroup m; // standard method to keep imu message. + + m.imu.clear(); + m.lio_time = meas.lidar_frame_end_time; + mtx_buffer.lock(); + while (!imu_buffer.empty()) { - auto pt = pcl[i]; - if (pcl[i].curvature < max_offs_time_ms) - { - pt.curvature += (frame_header_time - meas.last_lio_update_time) * 1000.0f; - meas.pcl_proc_cur->points.push_back(pt); - } - else - { - pt.curvature += (frame_header_time - m.lio_time) * 1000.0f; - meas.pcl_proc_next->points.push_back(pt); - } + if (imu_buffer.front()->header.stamp.toSec() > meas.lidar_frame_end_time) + break; + m.imu.push_back(imu_buffer.front()); + imu_buffer.pop_front(); } lid_raw_data_buffer.pop_front(); lid_header_time_buffer.pop_front(); - } - - meas.measures.push_back(m); - meas.lio_vio_flg = LIO; - // meas.last_lio_update_time = m.lio_time; - // printf("!!! meas.lio_vio_flg: %d \n", meas.lio_vio_flg); - // printf("[ Data Cut ] pcl_proc_cur number: %d \n", meas.pcl_proc_cur - // ->points.size()); printf("[ Data Cut ] LIO process time: %lf \n", - // omp_get_wtime() - t0); - return true; + mtx_buffer.unlock(); + sig_buffer.notify_all(); + + meas.lio_vio_flg = LIO; // process lidar topic, so timestamp should be lidar scan end. + meas.measures.push_back(m); + // ROS_INFO("ONlY HAS LiDAR and IMU, NO IMAGE!"); + lidar_pushed = false; // sync one whole lidar scan. + return true; + + break; } - case LIO: + case LIVO: { - double img_capture_time = img_time_buffer.front() + exposure_time_init; - meas.lio_vio_flg = VIO; - // printf("[ Data Cut ] VIO \n"); - meas.measures.clear(); - double imu_time = imu_buffer.front()->header.stamp.toSec(); - - struct MeasureGroup m; - m.vio_time = img_capture_time; - m.lio_time = meas.last_lio_update_time; - m.img = img_buffer.front(); - mtx_buffer.lock(); - // while ((!imu_buffer.empty() && (imu_time < img_capture_time))) - // { - // imu_time = imu_buffer.front()->header.stamp.toSec(); - // if (imu_time > img_capture_time) break; - // m.imu.push_back(imu_buffer.front()); - // imu_buffer.pop_front(); - // printf("[ Data Cut ] imu time: %lf \n", - // imu_buffer.front()->header.stamp.toSec()); - // } - img_buffer.pop_front(); - img_time_buffer.pop_front(); - mtx_buffer.unlock(); - sig_buffer.notify_all(); - meas.measures.push_back(m); - lidar_pushed = false; // after VIO update, the _lidar_frame_end_time will be refresh. - // printf("[ Data Cut ] VIO process time: %lf \n", omp_get_wtime() - t0); - return true; + /*** For LIVO mode, the time of LIO update is set to be the same as VIO, LIO + * first than VIO imediatly ***/ + EKF_STATE last_lio_vio_flg = meas.lio_vio_flg; + // double t0 = omp_get_wtime(); + switch (last_lio_vio_flg) + { + // double img_capture_time = meas.lidar_frame_beg_time + exposure_time_init; + case WAIT: + case VIO: + { + // printf("!!! meas.lio_vio_flg: %d \n", meas.lio_vio_flg); + double img_capture_time = img_time_buffer.front() + exposure_time_init; + /*** has img topic, but img topic timestamp larger than lidar end time, + * process lidar topic. After LIO update, the meas.lidar_frame_end_time + * will be refresh. ***/ + if (meas.last_lio_update_time < 0.0) + meas.last_lio_update_time = lid_header_time_buffer.front(); + // printf("[ Data Cut ] wait \n"); + // printf("[ Data Cut ] last_lio_update_time: %lf \n", + // meas.last_lio_update_time); + + double lid_newest_time = lid_header_time_buffer.back() + lid_raw_data_buffer.back()->points.back().curvature / double(1000); + double imu_newest_time = imu_buffer.back()->header.stamp.toSec(); + + if (img_capture_time < meas.last_lio_update_time + 0.00001) + { + img_buffer.pop_front(); + if(dynamic_slam && (img_buffer.size() == mask_buffer.size())) + { + mask_buffer.pop_front(); + } + img_time_buffer.pop_front(); + ROS_ERROR("[ Data Cut ] Throw one image frame! \n"); + return false; + } + + if (img_capture_time > lid_newest_time || img_capture_time > imu_newest_time) + { + // ROS_ERROR("lost first camera frame"); + // printf("img_capture_time, lid_newest_time, imu_newest_time: %lf , %lf + // , %lf \n", img_capture_time, lid_newest_time, imu_newest_time); + return false; + } + + struct MeasureGroup m; + + // printf("[ Data Cut ] LIO \n"); + // printf("[ Data Cut ] img_capture_time: %lf \n", img_capture_time); + m.imu.clear(); + m.lio_time = img_capture_time; + mtx_buffer.lock(); + while (!imu_buffer.empty()) + { + if (imu_buffer.front()->header.stamp.toSec() > m.lio_time) + break; + + if (imu_buffer.front()->header.stamp.toSec() > meas.last_lio_update_time) + m.imu.push_back(imu_buffer.front()); + + imu_buffer.pop_front(); + // printf("[ Data Cut ] imu time: %lf \n", + // imu_buffer.front()->header.stamp.toSec()); + } + mtx_buffer.unlock(); + sig_buffer.notify_all(); + + *(meas.pcl_proc_cur) = *(meas.pcl_proc_next); + PointCloudXYZI().swap(*meas.pcl_proc_next); + + int lid_frame_num = lid_raw_data_buffer.size(); + int max_size = meas.pcl_proc_cur->size() + 24000 * lid_frame_num; + meas.pcl_proc_cur->reserve(max_size); + meas.pcl_proc_next->reserve(max_size); + // deque lidar_buffer_tmp; + + while (!lid_raw_data_buffer.empty()) + { + if (lid_header_time_buffer.front() > img_capture_time) + break; + auto pcl(lid_raw_data_buffer.front()->points); + double frame_header_time(lid_header_time_buffer.front()); + float max_offs_time_ms = (m.lio_time - frame_header_time) * 1000.0f; + + for (int i = 0; i < pcl.size(); i++) + { + auto pt = pcl[i]; + if (pcl[i].curvature < max_offs_time_ms) + { + pt.curvature += (frame_header_time - meas.last_lio_update_time) * 1000.0f; + meas.pcl_proc_cur->points.push_back(pt); + } + else + { + pt.curvature += (frame_header_time - m.lio_time) * 1000.0f; + meas.pcl_proc_next->points.push_back(pt); + } + } + lid_raw_data_buffer.pop_front(); + lid_header_time_buffer.pop_front(); + } + + meas.measures.push_back(m); + meas.lio_vio_flg = LIO; + // meas.last_lio_update_time = m.lio_time; + // printf("!!! meas.lio_vio_flg: %d \n", meas.lio_vio_flg); + // printf("[ Data Cut ] pcl_proc_cur number: %d \n", meas.pcl_proc_cur + // ->points.size()); printf("[ Data Cut ] LIO process time: %lf \n", + // omp_get_wtime() - t0); + return true; + } + + case LIO: + { + double img_capture_time = img_time_buffer.front() + exposure_time_init; + meas.lio_vio_flg = VIO; + // printf("[ Data Cut ] VIO \n"); + meas.measures.clear(); + double imu_time = imu_buffer.front()->header.stamp.toSec(); + + struct MeasureGroup m; + m.vio_time = img_capture_time; + m.lio_time = meas.last_lio_update_time; + m.img = img_buffer.front(); + if(dynamic_slam && (img_buffer.size() == mask_buffer.size())) + { + m.mask = mask_buffer.front(); + } + mtx_buffer.lock(); + // while ((!imu_buffer.empty() && (imu_time < img_capture_time))) + // { + // imu_time = imu_buffer.front()->header.stamp.toSec(); + // if (imu_time > img_capture_time) break; + // m.imu.push_back(imu_buffer.front()); + // imu_buffer.pop_front(); + // printf("[ Data Cut ] imu time: %lf \n", + // imu_buffer.front()->header.stamp.toSec()); + // } + if(dynamic_slam && (img_buffer.size() == mask_buffer.size())) + { + mask_buffer.pop_front(); + } + img_buffer.pop_front(); + + img_time_buffer.pop_front(); + mtx_buffer.unlock(); + sig_buffer.notify_all(); + meas.measures.push_back(m); + lidar_pushed = false; // after VIO update, the _lidar_frame_end_time will be refresh. + // printf("[ Data Cut ] VIO process time: %lf \n", omp_get_wtime() - t0); + return true; + } + + default: + { + // printf("!! WRONG EKF STATE !!"); + return false; + } + // return false; + } + break; } - default: + case ONLY_LO: { - // printf("!! WRONG EKF STATE !!"); - return false; + if (!lidar_pushed) + { + // If not in lidar scan, need to generate new meas + if (lid_raw_data_buffer.empty()) + return false; + meas.lidar = lid_raw_data_buffer.front(); // push the first lidar topic + meas.lidar_frame_beg_time = lid_header_time_buffer.front(); // generate lidar_beg_time + meas.lidar_frame_end_time = meas.lidar_frame_beg_time + meas.lidar->points.back().curvature / double(1000); // calc lidar scan end time + lidar_pushed = true; + } + struct MeasureGroup m; // standard method to keep imu message. + m.lio_time = meas.lidar_frame_end_time; + mtx_buffer.lock(); + lid_raw_data_buffer.pop_front(); + lid_header_time_buffer.pop_front(); + mtx_buffer.unlock(); + sig_buffer.notify_all(); + lidar_pushed = false; // sync one whole lidar scan. + meas.lio_vio_flg = LO; // process lidar topic, so timestamp should be lidar scan end. + meas.measures.push_back(m); + return true; + break; } - // return false; + + default: + { + printf("!! WRONG SLAM TYPE !!"); + return false; } - break; - } - - case ONLY_LO: - { - if (!lidar_pushed) - { - // If not in lidar scan, need to generate new meas - if (lid_raw_data_buffer.empty()) return false; - meas.lidar = lid_raw_data_buffer.front(); // push the first lidar topic - meas.lidar_frame_beg_time = lid_header_time_buffer.front(); // generate lidar_beg_time - meas.lidar_frame_end_time = meas.lidar_frame_beg_time + meas.lidar->points.back().curvature / double(1000); // calc lidar scan end time - lidar_pushed = true; } - struct MeasureGroup m; // standard method to keep imu message. - m.lio_time = meas.lidar_frame_end_time; - mtx_buffer.lock(); - lid_raw_data_buffer.pop_front(); - lid_header_time_buffer.pop_front(); - mtx_buffer.unlock(); - sig_buffer.notify_all(); - lidar_pushed = false; // sync one whole lidar scan. - meas.lio_vio_flg = LO; // process lidar topic, so timestamp should be lidar scan end. - meas.measures.push_back(m); - return true; - break; - } - - default: - { - printf("!! WRONG SLAM TYPE !!"); - return false; - } - } - ROS_ERROR("out sync"); + ROS_ERROR("out sync"); } void LIVMapper::publish_img_rgb(const image_transport::Publisher &pubImage, VIOManagerPtr vio_manager) { - cv::Mat img_rgb = vio_manager->img_cp; - cv_bridge::CvImage out_msg; - out_msg.header.stamp = ros::Time::now(); - // out_msg.header.frame_id = "camera_init"; - out_msg.encoding = sensor_msgs::image_encodings::BGR8; - out_msg.image = img_rgb; - pubImage.publish(out_msg.toImageMsg()); + cv::Mat img_rgb = vio_manager->img_cp; + cv_bridge::CvImage out_msg; + out_msg.header.stamp = ros::Time::now(); + // out_msg.header.frame_id = "camera_init"; + out_msg.encoding = sensor_msgs::image_encodings::BGR8; + out_msg.image = img_rgb; + pubImage.publish(out_msg.toImageMsg()); } void LIVMapper::publish_frame_world(const ros::Publisher &pubLaserCloudFullRes, VIOManagerPtr vio_manager) { - if (pcl_w_wait_pub->empty()) return; - PointCloudXYZRGB::Ptr laserCloudWorldRGB(new PointCloudXYZRGB()); - if (img_en) - { - static int pub_num = 1; - *pcl_wait_pub += *pcl_w_wait_pub; - if(pub_num == pub_scan_num) + if (pcl_w_wait_pub->empty()) + return; + PointCloudXYZRGB::Ptr laserCloudWorldRGB(new PointCloudXYZRGB()); + if (img_en) { - pub_num = 1; - size_t size = pcl_wait_pub->points.size(); - laserCloudWorldRGB->reserve(size); - // double inv_expo = _state.inv_expo_time; - cv::Mat img_rgb = vio_manager->img_rgb; - for (size_t i = 0; i < size; i++) - { - PointTypeRGB pointRGB; - pointRGB.x = pcl_wait_pub->points[i].x; - pointRGB.y = pcl_wait_pub->points[i].y; - pointRGB.z = pcl_wait_pub->points[i].z; - - V3D p_w(pcl_wait_pub->points[i].x, pcl_wait_pub->points[i].y, pcl_wait_pub->points[i].z); - V3D pf(vio_manager->new_frame_->w2f(p_w)); if (pf[2] < 0) continue; - V2D pc(vio_manager->new_frame_->w2c(p_w)); - - if (vio_manager->new_frame_->cam_->isInFrame(pc.cast(), 3)) // 100 + static int pub_num = 1; + *pcl_wait_pub += *pcl_w_wait_pub; + if (pub_num == pub_scan_num) { - V3F pixel = vio_manager->getInterpolatedPixel(img_rgb, pc); - pointRGB.r = pixel[2]; - pointRGB.g = pixel[1]; - pointRGB.b = pixel[0]; - // pointRGB.r = pixel[2] * inv_expo; pointRGB.g = pixel[1] * inv_expo; pointRGB.b = pixel[0] * inv_expo; - // if (pointRGB.r > 255) pointRGB.r = 255; - // else if (pointRGB.r < 0) pointRGB.r = 0; - // if (pointRGB.g > 255) pointRGB.g = 255; - // else if (pointRGB.g < 0) pointRGB.g = 0; - // if (pointRGB.b > 255) pointRGB.b = 255; - // else if (pointRGB.b < 0) pointRGB.b = 0; - if (pf.norm() > blind_rgb_points) laserCloudWorldRGB->push_back(pointRGB); + pub_num = 1; + size_t size = pcl_wait_pub->points.size(); + laserCloudWorldRGB->reserve(size); + // double inv_expo = _state.inv_expo_time; + cv::Mat img_rgb = vio_manager->img_rgb; + for (size_t i = 0; i < size; i++) + { + PointTypeRGB pointRGB; + pointRGB.x = pcl_wait_pub->points[i].x; + pointRGB.y = pcl_wait_pub->points[i].y; + pointRGB.z = pcl_wait_pub->points[i].z; + + V3D p_w(pcl_wait_pub->points[i].x, pcl_wait_pub->points[i].y, pcl_wait_pub->points[i].z); + V3D pf(vio_manager->new_frame_->w2f(p_w)); + if (pf[2] < 0) + continue; + V2D pc(vio_manager->new_frame_->w2c(p_w)); + + if (vio_manager->new_frame_->cam_->isInFrame(pc.cast(), 3)) // 100 + { + V3F pixel = vio_manager->getInterpolatedPixel(img_rgb, pc); + pointRGB.r = pixel[2]; + pointRGB.g = pixel[1]; + pointRGB.b = pixel[0]; + // pointRGB.r = pixel[2] * inv_expo; pointRGB.g = pixel[1] * inv_expo; pointRGB.b = pixel[0] * inv_expo; + // if (pointRGB.r > 255) pointRGB.r = 255; + // else if (pointRGB.r < 0) pointRGB.r = 0; + // if (pointRGB.g > 255) pointRGB.g = 255; + // else if (pointRGB.g < 0) pointRGB.g = 0; + // if (pointRGB.b > 255) pointRGB.b = 255; + // else if (pointRGB.b < 0) pointRGB.b = 0; + if (pf.norm() > blind_rgb_points) + laserCloudWorldRGB->push_back(pointRGB); + } + } + } + else + { + pub_num++; } - } - } - else - { - pub_num++; } - } - - /*** Publish Frame ***/ - sensor_msgs::PointCloud2 laserCloudmsg; - if (img_en) - { - // cout << "RGB pointcloud size: " << laserCloudWorldRGB->size() << endl; - pcl::toROSMsg(*laserCloudWorldRGB, laserCloudmsg); - } - else - { - pcl::toROSMsg(*pcl_w_wait_pub, laserCloudmsg); - } - laserCloudmsg.header.stamp = ros::Time::now(); //.fromSec(last_timestamp_lidar); - laserCloudmsg.header.frame_id = "camera_init"; - pubLaserCloudFullRes.publish(laserCloudmsg); - - /**************** save map ****************/ - /* 1. make sure you have enough memories - /* 2. noted that pcd save will influence the real-time performences **/ - if (pcd_save_en) - { - int size = feats_undistort->points.size(); - PointCloudXYZI::Ptr laserCloudWorld(new PointCloudXYZI(size, 1)); - static int scan_wait_num = 0; + /*** Publish Frame ***/ + sensor_msgs::PointCloud2 laserCloudmsg; if (img_en) { - *pcl_wait_save += *laserCloudWorldRGB; + // cout << "RGB pointcloud size: " << laserCloudWorldRGB->size() << endl; + pcl::toROSMsg(*laserCloudWorldRGB, laserCloudmsg); } else { - *pcl_wait_save_intensity += *pcl_w_wait_pub; + pcl::toROSMsg(*pcl_w_wait_pub, laserCloudmsg); } - scan_wait_num++; + laserCloudmsg.header.stamp = ros::Time::now(); //.fromSec(last_timestamp_lidar); + laserCloudmsg.header.frame_id = "camera_init"; + pubLaserCloudFullRes.publish(laserCloudmsg); - if ((pcl_wait_save->size() > 0 || pcl_wait_save_intensity->size() > 0) && pcd_save_interval > 0 && scan_wait_num >= pcd_save_interval) + /**************** save map ****************/ + /* 1. make sure you have enough memories + /* 2. noted that pcd save will influence the real-time performences **/ + if (pcd_save_en) { - pcd_index++; - string all_points_dir(string(string(ROOT_DIR) + "Log/PCD/") + to_string(pcd_index) + string(".pcd")); - pcl::PCDWriter pcd_writer; - if (pcd_save_en) - { - cout << "current scan saved to /PCD/" << all_points_dir << endl; + int size = feats_undistort->points.size(); + PointCloudXYZI::Ptr laserCloudWorld(new PointCloudXYZI(size, 1)); + static int scan_wait_num = 0; + if (img_en) { - pcd_writer.writeBinary(all_points_dir, *pcl_wait_save); // pcl::io::savePCDFileASCII(all_points_dir, *pcl_wait_save); - PointCloudXYZRGB().swap(*pcl_wait_save); + *pcl_wait_save += *laserCloudWorldRGB; } else { - pcd_writer.writeBinary(all_points_dir, *pcl_wait_save_intensity); - PointCloudXYZI().swap(*pcl_wait_save_intensity); - } - Eigen::Quaterniond q(_state.rot_end); - fout_pcd_pos << _state.pos_end[0] << " " << _state.pos_end[1] << " " << _state.pos_end[2] << " " << q.w() << " " << q.x() << " " << q.y() - << " " << q.z() << " " << endl; - scan_wait_num = 0; - } + *pcl_wait_save_intensity += *pcl_w_wait_pub; + } + scan_wait_num++; + + if ((pcl_wait_save->size() > 0 || pcl_wait_save_intensity->size() > 0) && pcd_save_interval > 0 && scan_wait_num >= pcd_save_interval) + { + pcd_index++; + string all_points_dir(string(string(ROOT_DIR) + "Log/PCD/") + to_string(pcd_index) + string(".pcd")); + pcl::PCDWriter pcd_writer; + if (pcd_save_en) + { + cout << "current scan saved to /PCD/" << all_points_dir << endl; + if (img_en) + { + pcd_writer.writeBinary(all_points_dir, *pcl_wait_save); // pcl::io::savePCDFileASCII(all_points_dir, *pcl_wait_save); + PointCloudXYZRGB().swap(*pcl_wait_save); + } + else + { + pcd_writer.writeBinary(all_points_dir, *pcl_wait_save_intensity); + PointCloudXYZI().swap(*pcl_wait_save_intensity); + } + Eigen::Quaterniond q(_state.rot_end); + fout_pcd_pos << _state.pos_end[0] << " " << _state.pos_end[1] << " " << _state.pos_end[2] << " " << q.w() << " " << q.x() << " " << q.y() + << " " << q.z() << " " << endl; + scan_wait_num = 0; + } + } } - } - if(laserCloudWorldRGB->size() > 0) PointCloudXYZI().swap(*pcl_wait_pub); - PointCloudXYZI().swap(*pcl_w_wait_pub); + if (laserCloudWorldRGB->size() > 0) + PointCloudXYZI().swap(*pcl_wait_pub); + PointCloudXYZI().swap(*pcl_w_wait_pub); } void LIVMapper::publish_visual_sub_map(const ros::Publisher &pubSubVisualMap) { - PointCloudXYZI::Ptr laserCloudFullRes(visual_sub_map); - int size = laserCloudFullRes->points.size(); if (size == 0) return; - PointCloudXYZI::Ptr sub_pcl_visual_map_pub(new PointCloudXYZI()); - *sub_pcl_visual_map_pub = *laserCloudFullRes; - if (1) - { - sensor_msgs::PointCloud2 laserCloudmsg; - pcl::toROSMsg(*sub_pcl_visual_map_pub, laserCloudmsg); - laserCloudmsg.header.stamp = ros::Time::now(); - laserCloudmsg.header.frame_id = "camera_init"; - pubSubVisualMap.publish(laserCloudmsg); - } + PointCloudXYZI::Ptr laserCloudFullRes(visual_sub_map); + int size = laserCloudFullRes->points.size(); + if (size == 0) + return; + PointCloudXYZI::Ptr sub_pcl_visual_map_pub(new PointCloudXYZI()); + *sub_pcl_visual_map_pub = *laserCloudFullRes; + if (1) + { + sensor_msgs::PointCloud2 laserCloudmsg; + pcl::toROSMsg(*sub_pcl_visual_map_pub, laserCloudmsg); + laserCloudmsg.header.stamp = ros::Time::now(); + laserCloudmsg.header.frame_id = "camera_init"; + pubSubVisualMap.publish(laserCloudmsg); + } } void LIVMapper::publish_effect_world(const ros::Publisher &pubLaserCloudEffect, const std::vector &ptpl_list) { - int effect_feat_num = ptpl_list.size(); - PointCloudXYZI::Ptr laserCloudWorld(new PointCloudXYZI(effect_feat_num, 1)); - for (int i = 0; i < effect_feat_num; i++) - { - laserCloudWorld->points[i].x = ptpl_list[i].point_w_[0]; - laserCloudWorld->points[i].y = ptpl_list[i].point_w_[1]; - laserCloudWorld->points[i].z = ptpl_list[i].point_w_[2]; - } - sensor_msgs::PointCloud2 laserCloudFullRes3; - pcl::toROSMsg(*laserCloudWorld, laserCloudFullRes3); - laserCloudFullRes3.header.stamp = ros::Time::now(); - laserCloudFullRes3.header.frame_id = "camera_init"; - pubLaserCloudEffect.publish(laserCloudFullRes3); + int effect_feat_num = ptpl_list.size(); + PointCloudXYZI::Ptr laserCloudWorld(new PointCloudXYZI(effect_feat_num, 1)); + for (int i = 0; i < effect_feat_num; i++) + { + laserCloudWorld->points[i].x = ptpl_list[i].point_w_[0]; + laserCloudWorld->points[i].y = ptpl_list[i].point_w_[1]; + laserCloudWorld->points[i].z = ptpl_list[i].point_w_[2]; + } + sensor_msgs::PointCloud2 laserCloudFullRes3; + pcl::toROSMsg(*laserCloudWorld, laserCloudFullRes3); + laserCloudFullRes3.header.stamp = ros::Time::now(); + laserCloudFullRes3.header.frame_id = "camera_init"; + pubLaserCloudEffect.publish(laserCloudFullRes3); } -template void LIVMapper::set_posestamp(T &out) +template +void LIVMapper::set_posestamp(T &out) { - out.position.x = _state.pos_end(0); - out.position.y = _state.pos_end(1); - out.position.z = _state.pos_end(2); - out.orientation.x = geoQuat.x; - out.orientation.y = geoQuat.y; - out.orientation.z = geoQuat.z; - out.orientation.w = geoQuat.w; + out.position.x = _state.pos_end(0); + out.position.y = _state.pos_end(1); + out.position.z = _state.pos_end(2); + out.orientation.x = geoQuat.x; + out.orientation.y = geoQuat.y; + out.orientation.z = geoQuat.z; + out.orientation.w = geoQuat.w; } void LIVMapper::publish_odometry(const ros::Publisher &pubOdomAftMapped) { - odomAftMapped.header.frame_id = "camera_init"; - odomAftMapped.child_frame_id = "aft_mapped"; - odomAftMapped.header.stamp = ros::Time::now(); //.ros::Time()fromSec(last_timestamp_lidar); - set_posestamp(odomAftMapped.pose.pose); - - static tf::TransformBroadcaster br; - tf::Transform transform; - tf::Quaternion q; - transform.setOrigin(tf::Vector3(_state.pos_end(0), _state.pos_end(1), _state.pos_end(2))); - q.setW(geoQuat.w); - q.setX(geoQuat.x); - q.setY(geoQuat.y); - q.setZ(geoQuat.z); - transform.setRotation(q); - br.sendTransform( tf::StampedTransform(transform, odomAftMapped.header.stamp, "camera_init", "aft_mapped") ); - pubOdomAftMapped.publish(odomAftMapped); + odomAftMapped.header.frame_id = "camera_init"; + odomAftMapped.child_frame_id = "aft_mapped"; + odomAftMapped.header.stamp = ros::Time::now(); //.ros::Time()fromSec(last_timestamp_lidar); + set_posestamp(odomAftMapped.pose.pose); + + static tf::TransformBroadcaster br; + tf::Transform transform; + tf::Quaternion q; + transform.setOrigin(tf::Vector3(_state.pos_end(0), _state.pos_end(1), _state.pos_end(2))); + q.setW(geoQuat.w); + q.setX(geoQuat.x); + q.setY(geoQuat.y); + q.setZ(geoQuat.z); + transform.setRotation(q); + br.sendTransform(tf::StampedTransform(transform, odomAftMapped.header.stamp, "camera_init", "aft_mapped")); + pubOdomAftMapped.publish(odomAftMapped); } void LIVMapper::publish_mavros(const ros::Publisher &mavros_pose_publisher) { - msg_body_pose.header.stamp = ros::Time::now(); - msg_body_pose.header.frame_id = "camera_init"; - set_posestamp(msg_body_pose.pose); - mavros_pose_publisher.publish(msg_body_pose); + msg_body_pose.header.stamp = ros::Time::now(); + msg_body_pose.header.frame_id = "camera_init"; + set_posestamp(msg_body_pose.pose); + mavros_pose_publisher.publish(msg_body_pose); } void LIVMapper::publish_path(const ros::Publisher pubPath) { - set_posestamp(msg_body_pose.pose); - msg_body_pose.header.stamp = ros::Time::now(); - msg_body_pose.header.frame_id = "camera_init"; - path.poses.push_back(msg_body_pose); - pubPath.publish(path); + set_posestamp(msg_body_pose.pose); + msg_body_pose.header.stamp = ros::Time::now(); + msg_body_pose.header.frame_id = "camera_init"; + path.poses.push_back(msg_body_pose); + pubPath.publish(path); } \ No newline at end of file diff --git a/src/frame.cpp b/src/frame.cpp index 2a0b1268..b018a4cf 100644 --- a/src/frame.cpp +++ b/src/frame.cpp @@ -28,6 +28,13 @@ Frame::Frame(vk::AbstractCamera *cam, const cv::Mat &img) initFrame(img); } +Frame::Frame(vk::AbstractCamera *cam, const cv::Mat &img, const cv::Mat &mask) + : id_(frame_counter_++), + cam_(cam) +{ + initFrame(img, mask); +} + Frame::~Frame() { std::for_each(fts_.begin(), fts_.end(), [&](Feature *i) { delete i; }); @@ -37,6 +44,27 @@ void Frame::initFrame(const cv::Mat &img) { if (img.empty()) { throw std::runtime_error("Frame: provided image is empty"); } + + std::cout << "img.cols: " << img.cols << ", img.rows: " << img.rows << std::endl; + std::cout << "cam.width: " << cam_->width() << ", cam.height: " << cam_->height() << std::endl; + if (img.cols != cam_->width() || img.rows != cam_->height()) + { + throw std::runtime_error("Frame: provided image has not the same size as the camera model"); + } + + if (img.type() != CV_8UC1) { throw std::runtime_error("Frame: provided image is not grayscale"); } + + img_ = img; +} + + +void Frame::initFrame(const cv::Mat &img, const cv::Mat &mask) +{ + if (img.empty()) { throw std::runtime_error("Frame: provided image is empty"); } + + + std::cout << "img.cols: " << img.cols << ", img.rows: " << img.rows << std::endl; + std::cout << "cam.width: " << cam_->width() << ", cam.height: " << cam_->height() << std::endl; if (img.cols != cam_->width() || img.rows != cam_->height()) { throw std::runtime_error("Frame: provided image has not the same size as the camera model"); @@ -45,6 +73,7 @@ void Frame::initFrame(const cv::Mat &img) if (img.type() != CV_8UC1) { throw std::runtime_error("Frame: provided image is not grayscale"); } img_ = img; + mask_ = mask; } /// Utility functions for the Frame class diff --git a/src/preprocess.cpp b/src/preprocess.cpp index afe34149..50b2d825 100755 --- a/src/preprocess.cpp +++ b/src/preprocess.cpp @@ -85,6 +85,10 @@ void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointClo robosense_handler(msg); break; + case URBANLOCO: + urbanloco_handler(msg); + break; + default: printf("Error LiDAR Type: %d \n", lidar_type); break; @@ -511,6 +515,110 @@ void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) // pub_func(pl_surf, pub_corn, msg->header.stamp); } +void Preprocess::urbanloco_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) +{ + pl_surf.clear(); + pl_corn.clear(); + pl_full.clear(); + + pcl::PointCloud pl_orig; + pcl::fromROSMsg(*msg, pl_orig); + int plsize = pl_orig.points.size(); + std::cout << "[UrbanlocoHandler] plsize: " << plsize << std::endl; + if (plsize == 0) + return; + pl_surf.reserve(plsize); + + bool is_first[32]; + double yaw_fp[32] = {0}; // yaw of first scan point + double omega_l = 3.61; // scan angular velocity + float yaw_last[32] = {0.0}; // yaw of last scan point + float time_last[32] = {0.0}; // last offset time + + given_offset_time = false; + memset(is_first, true, sizeof(is_first)); + double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578; + double yaw_end = yaw_first; + int layer_first = pl_orig.points[0].ring; + std::cout << "layer_first: " << layer_first << std::endl; + for (uint i = plsize - 1; i > 0; i--) + { + if (pl_orig.points[i].ring == layer_first) + { + yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578; + break; + } + } + std::cout << "yaw_end: " << yaw_end << std::endl; + for (int i = 0; i < plsize; i++) + { + PointType added_pt; + + added_pt.normal_x = 0; + added_pt.normal_y = 0; + added_pt.normal_z = 0; + added_pt.x = pl_orig.points[i].x; + added_pt.y = pl_orig.points[i].y; + added_pt.z = pl_orig.points[i].z; + added_pt.intensity = pl_orig.points[i].intensity; + + if (!given_offset_time) + { + int layer = pl_orig.points[i].ring; + double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957; + if(layer > 31) + { + continue; + } + if (is_first[layer]) + { + // printf("layer: %d; is first: %d", layer, is_first[layer]); + yaw_fp[layer] = yaw_angle; + is_first[layer] = false; + added_pt.curvature = 0.0; + yaw_last[layer] = yaw_angle; + time_last[layer] = added_pt.curvature; + continue; + } + + // compute offset time + if (yaw_angle <= yaw_fp[layer]) + { + added_pt.curvature = (yaw_fp[layer] - yaw_angle) / omega_l; + } + else + { + added_pt.curvature = (yaw_fp[layer] - yaw_angle + 360.0) / omega_l; + } + + if (added_pt.curvature < time_last[layer]) + added_pt.curvature += 360.0 / omega_l; + + // added_pt.curvature = pl_orig.points[i].t; + + yaw_last[layer] = yaw_angle; + time_last[layer] = added_pt.curvature; + } + + // if(i==(plsize-1)) printf("index: %d layer: %d, yaw: %lf, offset-time: + // %lf, condition: %d\n", i, layer, yaw_angle, added_pt.curvature, + // prints); + if (i % point_filter_num == 0) + { + if (added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z > blind) + { + pl_surf.points.push_back(added_pt); + // printf("time mode: %d time: %d \n", given_offset_time, + // pl_orig.points[i].t); + } + } + } + std::cout << "[UrbanLocoHandler] end. " << std::endl; + // pub_func(pl_surf, pub_full, msg->header.stamp); + // pub_func(pl_surf, pub_surf, msg->header.stamp); + // pub_func(pl_surf, pub_corn, msg->header.stamp); +} + void Preprocess::Pandar128_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) { pl_surf.clear(); diff --git a/src/vio.cpp b/src/vio.cpp index 2ff8fd0f..2e525986 100755 --- a/src/vio.cpp +++ b/src/vio.cpp @@ -780,6 +780,442 @@ void VIOManager::retrieveFromVisualSparseMap(cv::Mat img, vector & // "< &pg, const unordered_map &plane_map) +{ + if (feat_map.size() <= 0) return; + double ts0 = omp_get_wtime(); + + // pg_down->reserve(feat_map.size()); + // downSizeFilter.setInputCloud(pg); + // downSizeFilter.filter(*pg_down); + + // resetRvizDisplay(); + visual_submap->reset(); + + // Controls whether to include the visual submap from the previous frame. + sub_feat_map.clear(); + + float voxel_size = 0.5; + + if (!normal_en) warp_map.clear(); + + cv::Mat depth_img = cv::Mat::zeros(height, width, CV_32FC1); + float *it = (float *)depth_img.data; + + // float it[height * width] = {0.0}; + + // double t_insert, t_depth, t_position; + // t_insert=t_depth=t_position=0; + + int loc_xyz[3]; + + // printf("A0. initial depthmap: %.6lf \n", omp_get_wtime() - ts0); + // double ts1 = omp_get_wtime(); + + // printf("pg size: %zu \n", pg.size()); + + for (int i = 0; i < pg.size(); i++) + { + // double t0 = omp_get_wtime(); + + V3D pt_w = pg[i].point_w; + + for (int j = 0; j < 3; j++) + { + loc_xyz[j] = floor(pt_w[j] / voxel_size); + if (loc_xyz[j] < 0) { loc_xyz[j] -= 1.0; } + } + VOXEL_LOCATION position(loc_xyz[0], loc_xyz[1], loc_xyz[2]); + + // t_position += omp_get_wtime()-t0; + // double t1 = omp_get_wtime(); + + auto iter = sub_feat_map.find(position); + if (iter == sub_feat_map.end()) { sub_feat_map[position] = 0; } + else { iter->second = 0; } + + // t_insert += omp_get_wtime()-t1; + // double t2 = omp_get_wtime(); + + V3D pt_c(new_frame_->w2f(pt_w)); + + if (pt_c[2] > 0) + { + V2D px; + // px[0] = fx * pt_c[0]/pt_c[2] + cx; + // px[1] = fy * pt_c[1]/pt_c[2]+ cy; + px = new_frame_->cam_->world2cam(pt_c); + + if (new_frame_->cam_->isInFrame(px.cast(), border)) + { + // cv::circle(img_cp, cv::Point2f(px[0], px[1]), 3, cv::Scalar(0, 0, 255), -1, 8); + if(mask.at(px[1], px[0])) + { + float depth = pt_c[2]; + int col = int(px[0]); + int row = int(px[1]); + it[width * row + col] = depth; + } + + } + } + // t_depth += omp_get_wtime()-t2; + } + + // imshow("depth_img", depth_img); + // printf("A1: %.6lf \n", omp_get_wtime() - ts1); + // printf("A11. calculate pt position: %.6lf \n", t_position); + // printf("A12. sub_postion.insert(position): %.6lf \n", t_insert); + // printf("A13. generate depth map: %.6lf \n", t_depth); + // printf("A. projection: %.6lf \n", omp_get_wtime() - ts0); + + // double t1 = omp_get_wtime(); + vector DeleteKeyList; + + for (auto &iter : sub_feat_map) + { + VOXEL_LOCATION position = iter.first; + + // double t4 = omp_get_wtime(); + auto corre_voxel = feat_map.find(position); + // double t5 = omp_get_wtime(); + + if (corre_voxel != feat_map.end()) + { + bool voxel_in_fov = false; + std::vector &voxel_points = corre_voxel->second->voxel_points; + int voxel_num = voxel_points.size(); + + for (int i = 0; i < voxel_num; i++) + { + VisualPoint *pt = voxel_points[i]; + if (pt == nullptr) continue; + if (pt->obs_.size() == 0) continue; + + V3D norm_vec(new_frame_->T_f_w_.rotation_matrix() * pt->normal_); + V3D dir(new_frame_->T_f_w_ * pt->pos_); + if (dir[2] < 0) continue; + // dir.normalize(); + // if (dir.dot(norm_vec) <= 0.17) continue; // 0.34 70 degree 0.17 80 degree 0.08 85 degree + + V2D pc(new_frame_->w2c(pt->pos_)); + if (new_frame_->cam_->isInFrame(pc.cast(), border)) + { + // cv::circle(img_cp, cv::Point2f(pc[0], pc[1]), 3, cv::Scalar(0, 255, 255), -1, 8); + voxel_in_fov = true; + int index = static_cast(pc[1] / grid_size) * grid_n_width + static_cast(pc[0] / grid_size); + grid_num[index] = TYPE_MAP; + Vector3d obs_vec(new_frame_->pos() - pt->pos_); + float cur_dist = obs_vec.norm(); + if (cur_dist <= map_dist[index]) + { + map_dist[index] = cur_dist; + retrieve_voxel_points[index] = pt; + } + } + } + if (!voxel_in_fov) { DeleteKeyList.push_back(position); } + } + } + + // RayCasting Module + if (raycast_en) + { + for (int i = 0; i < length; i++) + { + if (grid_num[i] == TYPE_MAP || border_flag[i] == 1) continue; + + // int row = static_cast(i / grid_n_width) * grid_size + grid_size / + // 2; int col = (i - static_cast(i / grid_n_width) * grid_n_width) * + // grid_size + grid_size / 2; + + // cv::circle(img_cp, cv::Point2f(col, row), 3, cv::Scalar(255, 255, 0), + // -1, 8); + + // vector sample_points_temp; + // bool add_sample = false; + + for (const auto &it : rays_with_sample_points[i]) + { + V3D sample_point_w = new_frame_->f2w(it); + // sample_points_temp.push_back(sample_point_w); + + for (int j = 0; j < 3; j++) + { + loc_xyz[j] = floor(sample_point_w[j] / voxel_size); + if (loc_xyz[j] < 0) { loc_xyz[j] -= 1.0; } + } + + VOXEL_LOCATION sample_pos(loc_xyz[0], loc_xyz[1], loc_xyz[2]); + + auto corre_sub_feat_map = sub_feat_map.find(sample_pos); + if (corre_sub_feat_map != sub_feat_map.end()) break; + + auto corre_feat_map = feat_map.find(sample_pos); + if (corre_feat_map != feat_map.end()) + { + bool voxel_in_fov = false; + + std::vector &voxel_points = corre_feat_map->second->voxel_points; + int voxel_num = voxel_points.size(); + if (voxel_num == 0) continue; + + for (int j = 0; j < voxel_num; j++) + { + VisualPoint *pt = voxel_points[j]; + + if (pt == nullptr) continue; + if (pt->obs_.size() == 0) continue; + + // sub_map_ray.push_back(pt); // cloud_visual_sub_map + // add_sample = true; + + V3D norm_vec(new_frame_->T_f_w_.rotation_matrix() * pt->normal_); + V3D dir(new_frame_->T_f_w_ * pt->pos_); + if (dir[2] < 0) continue; + dir.normalize(); + // if (dir.dot(norm_vec) <= 0.17) continue; // 0.34 70 degree 0.17 80 degree 0.08 85 degree + + V2D pc(new_frame_->w2c(pt->pos_)); + + if (new_frame_->cam_->isInFrame(pc.cast(), border)) + { + // cv::circle(img_cp, cv::Point2f(pc[0], pc[1]), 3, cv::Scalar(255, 255, 0), -1, 8); + // sub_map_ray_fov.push_back(pt); + + voxel_in_fov = true; + int index = static_cast(pc[1] / grid_size) * grid_n_width + static_cast(pc[0] / grid_size); + grid_num[index] = TYPE_MAP; + Vector3d obs_vec(new_frame_->pos() - pt->pos_); + + float cur_dist = obs_vec.norm(); + + if (cur_dist <= map_dist[index]) + { + map_dist[index] = cur_dist; + retrieve_voxel_points[index] = pt; + } + } + } + + if (voxel_in_fov) sub_feat_map[sample_pos] = 0; + break; + } + else + { + VOXEL_LOCATION sample_pos(loc_xyz[0], loc_xyz[1], loc_xyz[2]); + auto iter = plane_map.find(sample_pos); + if (iter != plane_map.end()) + { + VoxelOctoTree *current_octo; + current_octo = iter->second->find_correspond(sample_point_w); + if (current_octo->plane_ptr_->is_plane_) + { + pointWithVar plane_center; + VoxelPlane &plane = *current_octo->plane_ptr_; + plane_center.point_w = plane.center_; + plane_center.normal = plane.normal_; + visual_submap->add_from_voxel_map.push_back(plane_center); + break; + } + } + } + } + // if(add_sample) sample_points.push_back(sample_points_temp); + } + } + + for (auto &key : DeleteKeyList) + { + sub_feat_map.erase(key); + } + + // double t2 = omp_get_wtime(); + + // cout<<"B. feat_map.find: "<w2c(pt->pos_)); + + // cv::circle(img_cp, cv::Point2f(pc[0], pc[1]), 3, cv::Scalar(0, 0, 255), -1, 8); // Green Sparse Align tracked + + V3D pt_cam(new_frame_->w2f(pt->pos_)); + bool depth_continous = false; + for (int u = -patch_size_half; u <= patch_size_half; u++) + { + for (int v = -patch_size_half; v <= patch_size_half; v++) + { + if (u == 0 && v == 0) continue; + + float depth = it[width * (v + int(pc[1])) + u + int(pc[0])]; + + if (depth == 0.) continue; + + double delta_dist = abs(pt_cam[2] - depth); + + if (delta_dist > 0.5) + { + depth_continous = true; + break; + } + } + if (depth_continous) break; + } + if (depth_continous) continue; + + // t_2 += omp_get_wtime() - t_1; + + // t_1 = omp_get_wtime(); + Feature *ref_ftr; + std::vector patch_wrap(warp_len); + + int search_level; + Matrix2d A_cur_ref_zero; + + if (!pt->is_normal_initialized_) continue; + + if (normal_en) + { + float phtometric_errors_min = std::numeric_limits::max(); + + if (pt->obs_.size() == 1) + { + ref_ftr = *pt->obs_.begin(); + pt->ref_patch = ref_ftr; + pt->has_ref_patch_ = true; + } + else if (!pt->has_ref_patch_) + { + for (auto it = pt->obs_.begin(), ite = pt->obs_.end(); it != ite; ++it) + { + Feature *ref_patch_temp = *it; + float *patch_temp = ref_patch_temp->patch_; + float phtometric_errors = 0.0; + int count = 0; + for (auto itm = pt->obs_.begin(), itme = pt->obs_.end(); itm != itme; ++itm) + { + if ((*itm)->id_ == ref_patch_temp->id_) continue; + float *patch_cache = (*itm)->patch_; + + for (int ind = 0; ind < patch_size_total; ind++) + { + phtometric_errors += (patch_temp[ind] - patch_cache[ind]) * (patch_temp[ind] - patch_cache[ind]); + } + count++; + } + phtometric_errors = phtometric_errors / count; + if (phtometric_errors < phtometric_errors_min) + { + phtometric_errors_min = phtometric_errors; + ref_ftr = ref_patch_temp; + } + } + pt->ref_patch = ref_ftr; + pt->has_ref_patch_ = true; + } + else { ref_ftr = pt->ref_patch; } + } + else + { + if (!pt->getCloseViewObs(new_frame_->pos(), ref_ftr, pc)) continue; + } + + if (normal_en) + { + V3D norm_vec = (ref_ftr->T_f_w_.rotation_matrix() * pt->normal_).normalized(); + + V3D pf(ref_ftr->T_f_w_ * pt->pos_); + // V3D pf_norm = pf.normalized(); + + // double cos_theta = norm_vec.dot(pf_norm); + // if(cos_theta < 0) norm_vec = -norm_vec; + // if (abs(cos_theta) < 0.08) continue; // 0.5 60 degree 0.34 70 degree 0.17 80 degree 0.08 85 degree + + SE3 T_cur_ref = new_frame_->T_f_w_ * ref_ftr->T_f_w_.inverse(); + + getWarpMatrixAffineHomography(*cam, ref_ftr->px_, pf, norm_vec, T_cur_ref, 0, A_cur_ref_zero); + + search_level = getBestSearchLevel(A_cur_ref_zero, 2); + } + else + { + auto iter_warp = warp_map.find(ref_ftr->id_); + if (iter_warp != warp_map.end()) + { + search_level = iter_warp->second->search_level; + A_cur_ref_zero = iter_warp->second->A_cur_ref; + } + else + { + getWarpMatrixAffine(*cam, ref_ftr->px_, ref_ftr->f_, (ref_ftr->pos() - pt->pos_).norm(), new_frame_->T_f_w_ * ref_ftr->T_f_w_.inverse(), + ref_ftr->level_, 0, patch_size_half, A_cur_ref_zero); + + search_level = getBestSearchLevel(A_cur_ref_zero, 2); + + Warp *ot = new Warp(search_level, A_cur_ref_zero); + warp_map[ref_ftr->id_] = ot; + } + } + // t_4 += omp_get_wtime() - t_1; + + // t_1 = omp_get_wtime(); + + for (int pyramid_level = 0; pyramid_level <= patch_pyrimid_level - 1; pyramid_level++) + { + warpAffine(A_cur_ref_zero, ref_ftr->img_, ref_ftr->px_, ref_ftr->level_, search_level, pyramid_level, patch_size_half, patch_wrap.data()); + } + + getImagePatch(img, pc, patch_buffer.data(), 0); + + float error = 0.0; + for (int ind = 0; ind < patch_size_total; ind++) + { + error += (ref_ftr->inv_expo_time_ * patch_wrap[ind] - state->inv_expo_time * patch_buffer[ind]) * + (ref_ftr->inv_expo_time_ * patch_wrap[ind] - state->inv_expo_time * patch_buffer[ind]); + } + + if (ncc_en) + { + double ncc = calculateNCC(patch_wrap.data(), patch_buffer.data(), patch_size_total); + if (ncc < ncc_thre) + { + // grid_num[i] = TYPE_UNKNOWN; + continue; + } + } + + if (error > outlier_threshold * patch_size_total) continue; + + visual_submap->voxel_points.push_back(pt); + visual_submap->propa_errors.push_back(error); + visual_submap->search_levels.push_back(search_level); + visual_submap->errors.push_back(error); + visual_submap->warp_patch.push_back(patch_wrap); + visual_submap->inv_expo_list.push_back(ref_ftr->inv_expo_time_); + + // t_5 += omp_get_wtime() - t_1; + } + } + total_points = visual_submap->voxel_points.size(); + + // double t3 = omp_get_wtime(); + // cout<<"C. addSubSparseMap: "< &pg, const unordered_map &feat_map, double img_time) +{ + if (width != img.cols || height != img.rows) + { + if (img.empty()) printf("[ VIO ] Empty Image!\n"); + cv::resize(img, img, cv::Size(img.cols * image_resize_factor, img.rows * image_resize_factor), 0, 0, CV_INTER_LINEAR); + } + img_rgb = img.clone(); + img_cp = img.clone(); + img_rgb = img.clone(); + img_cp = img.clone(); + mask_cp = mask.clone(); + mask_p = mask.clone(); + + // img_test = img.clone(); + + if (img.channels() == 3) cv::cvtColor(img, img, CV_BGR2GRAY); + + new_frame_.reset(new Frame(cam, img, mask)); + updateFrameState(*state); + + resetGrid(); + + double t1 = omp_get_wtime(); + + retrieveFromVisualSparseMap(img, mask, pg, feat_map); + + double t2 = omp_get_wtime(); + + computeJacobianAndUpdateEKF(img); + + double t3 = omp_get_wtime(); + + generateVisualMapPoints(img, pg); + + double t4 = omp_get_wtime(); + + plotTrackedPoints(); + + if (plot_flag) projectPatchFromRefToCur(feat_map); + + double t5 = omp_get_wtime(); + + updateVisualMapPoints(img); + + double t6 = omp_get_wtime(); + + updateReferencePatch(feat_map); + + double t7 = omp_get_wtime(); + + if(colmap_output_en) dumpDataForColmap(); + + frame_count++; + ave_total = ave_total * (frame_count - 1) / frame_count + (t7 - t1 - (t5 - t4)) / frame_count; + + // printf("[ VIO ] feat_map.size(): %zu\n", feat_map.size()); + // printf("\033[1;32m[ VIO time ]: current frame: retrieveFromVisualSparseMap time: %.6lf secs.\033[0m\n", t2 - t1); + // printf("\033[1;32m[ VIO time ]: current frame: computeJacobianAndUpdateEKF time: %.6lf secs, comp H: %.6lf secs, ekf: %.6lf secs.\033[0m\n", t3 - t2, computeH, ekf_time); + // printf("\033[1;32m[ VIO time ]: current frame: generateVisualMapPoints time: %.6lf secs.\033[0m\n", t4 - t3); + // printf("\033[1;32m[ VIO time ]: current frame: updateVisualMapPoints time: %.6lf secs.\033[0m\n", t6 - t5); + // printf("\033[1;32m[ VIO time ]: current frame: updateReferencePatch time: %.6lf secs.\033[0m\n", t7 - t6); + // printf("\033[1;32m[ VIO time ]: current total time: %.6lf, average total time: %.6lf secs.\033[0m\n", t7 - t1 - (t5 - t4), ave_total); + + // ave_build_residual_time = ave_build_residual_time * (frame_count - 1) / frame_count + (t2 - t1) / frame_count; + // ave_ekf_time = ave_ekf_time * (frame_count - 1) / frame_count + (t3 - t2) / frame_count; + + // cout << BLUE << "ave_build_residual_time: " << ave_build_residual_time << RESET << endl; + // cout << BLUE << "ave_ekf_time: " << ave_ekf_time << RESET << endl; + + printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n"); + printf("\033[1;34m| VIO Time |\033[0m\n"); + printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n"); + printf("\033[1;34m| %-29s | %-27zu |\033[0m\n", "Sparse Map Size", feat_map.size()); + printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n"); + printf("\033[1;34m| %-29s | %-27s |\033[0m\n", "Algorithm Stage", "Time (secs)"); + printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n"); + printf("\033[1;32m| %-29s | %-27lf |\033[0m\n", "retrieveFromVisualSparseMap", t2 - t1); + printf("\033[1;32m| %-29s | %-27lf |\033[0m\n", "computeJacobianAndUpdateEKF", t3 - t2); + printf("\033[1;32m| %-27s | %-27lf |\033[0m\n", "-> computeJacobian", compute_jacobian_time); + printf("\033[1;32m| %-27s | %-27lf |\033[0m\n", "-> updateEKF", update_ekf_time); + printf("\033[1;32m| %-29s | %-27lf |\033[0m\n", "generateVisualMapPoints", t4 - t3); + printf("\033[1;32m| %-29s | %-27lf |\033[0m\n", "updateVisualMapPoints", t6 - t5); + printf("\033[1;32m| %-29s | %-27lf |\033[0m\n", "updateReferencePatch", t7 - t6); + printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n"); + printf("\033[1;32m| %-29s | %-27lf |\033[0m\n", "Current Total Time", t7 - t1 - (t5 - t4)); + printf("\033[1;32m| %-29s | %-27lf |\033[0m\n", "Average Total Time", ave_total); + printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n"); + + // std::string text = std::to_string(int(1 / (t7 - t1 - (t5 - t4)))) + " HZ"; + // cv::Point2f origin; + // origin.x = 20; + // origin.y = 20; + // cv::putText(img_cp, text, origin, cv::FONT_HERSHEY_COMPLEX, 0.6, cv::Scalar(255, 255, 255), 1, 8, 0); + // cv::imwrite("/home/chunran/Desktop/raycasting/" + std::to_string(new_frame_->id_) + ".png", img_cp); +} + void VIOManager::processFrame(cv::Mat &img, vector &pg, const unordered_map &feat_map, double img_time) { if (width != img.cols || height != img.rows)