diff --git a/CMakeLists.txt b/CMakeLists.txt index 0652aab6..e59fc15f 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -87,24 +87,29 @@ find_package(catkin REQUIRED COMPONENTS tf message_generation eigen_conversions - vikit_common - vikit_ros cv_bridge image_transport ) +add_subdirectory(vikit_common) + +# glog +find_package(Glog REQUIRED) +include_directories(${GLOG_INCLUDE_DIR}) + +# gflags +find_package(Gflags REQUIRED) +include_directories(${GFLAGS_INCLUDE_DIR}) + find_package(Eigen3 REQUIRED) find_package(PCL REQUIRED) find_package(OpenCV REQUIRED) -find_package(Sophus REQUIRED) find_package(Boost REQUIRED COMPONENTS thread) -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 - DEPENDS EIGEN3 PCL OpenCV Sophus + CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs message_runtime cv_bridge image_transport + DEPENDS EIGEN3 PCL OpenCV ) # Include directories for dependencies @@ -113,12 +118,12 @@ include_directories( ${EIGEN3_INCLUDE_DIR} ${PCL_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} - ${Sophus_INCLUDE_DIRS} include + vikit_common/include ) # Add libraries -add_library(vio src/vio.cpp src/frame.cpp src/visual_point.cpp) +add_library(vio src/vio.cpp src/image_frame.cpp) add_library(lio src/voxel_map.cpp) add_library(pre src/preprocess.cpp) add_library(imu_proc src/IMU_Processing.cpp) @@ -137,8 +142,10 @@ target_link_libraries(fastlivo_mapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} - ${Sophus_LIBRARIES} + glog::glog + ${GFLAGS_LIBRARIES} ${Boost_LIBRARIES} + vikit_plugin ) # Link mimalloc if found diff --git a/Log/image/.gitkeep b/Log/image/.gitkeep deleted file mode 100644 index e69de29b..00000000 diff --git a/Log/pcd/.gitkeep b/Log/pcd/.gitkeep deleted file mode 100644 index e69de29b..00000000 diff --git a/README.md b/README.md index 6d7d2151..4b882c98 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,11 @@ # FAST-LIVO2 +## 改进内容 +1. 将sophus1.24.6和vikit整合至项目内，现在不再需要安装这两项依赖 +2. 添加了LRU内存管理，控制内存增长速度 +3. 使用谷歌风格对部分变量函数等进行了重命名，添加了部分注释 +4. 引入了glog和gflags（可按照后续步骤安装），在程序崩溃时可以便捷地找到出错的位置，便于调试，同时便于外部参数输入 +5. 添加了glibc malloc内存分配优化，避免长时间占用空闲内存(2025-09-03新增) +6. 删去了LO模式和若干冗余变量，主要处理流程更加清晰(2025-09-03新增) ## FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry @@ -60,44 +67,38 @@ Eigen>=3.3.4, Follow [Eigen Installation](https://eigen.tuxfamily.org/index.php? OpenCV>=4.2, Follow [Opencv Installation](http://opencv.org/). -### 2.3 Sophus - -Sophus Installation for the non-templated/double-only version. - -```bash -git clone https://github.com/strasdat/Sophus.git -cd Sophus -git checkout a621ff -mkdir build && cd build && cmake .. -make -sudo make install -``` - -### 2.4 Vikit - -Vikit contains camera models, some math and interpolation functions that we need. Vikit is a catkin project, therefore, download it into your catkin workspace source folder. - -```bash -# Different from the one used in fast-livo1 -cd catkin_ws/src -git clone https://github.com/xuankuzcr/rpg_vikit.git -``` - ## 3. Build Clone the repository and catkin_make: ``` cd ~/catkin_ws/src -git clone https://github.com/hku-mars/FAST-LIVO2 -cd ../ +git clone https://github.com/yqmy0814/FAST-LIVO2 + +# 安装glog和gflags,已安装则跳过 +cd FAST-LIVO2/thirdparty +tar -xvf gflags-2.2.2.tar.gz +cd gflags-2.2.2 +mkdir build && cd build +cmake -DBUILD_SHARED_LIBS=ON -DCMAKE_CXX_FLAGS=-fPIC .. +make -j4 +sudo make install +cd ../.. +tar -xvf glog-0.4.0.tar.gz +cd glog-0.4.0 +mkdir build && cd build +cmake -DBUILD_SHARED_LIBS=ON .. +make -j4 +sudo make install + +cd ~/catkin_ws catkin_make source ~/catkin_ws/devel/setup.bash ``` ## 4. Run our examples -Download our collected rosbag files via OneDrive ([**FAST-LIVO2-Dataset**](https://connecthkuhk-my.sharepoint.com/:f:/g/personal/zhengcr_connect_hku_hk/ErdFNQtjMxZOorYKDTtK4ugBkogXfq1OfDm90GECouuIQA?e=KngY9Z)). +Download FAST-LIVO2-Dataset from [Global-LVBA](https://github.com/xuankuzcr/Global-LVBA) Section IV. ``` roslaunch fast_livo mapping_avia.launch @@ -107,4 +108,4 @@ rosbag play YOUR_DOWNLOADED.bag ## 5. License -The source code of this package is released under the [**GPLv2**](http://www.gnu.org/licenses/) license. For commercial use, please contact me at and Prof. Fu Zhang at to discuss an alternative license. \ No newline at end of file +The source code of this package is released under the [**GPLv2**](http://www.gnu.org/licenses/) license. For commercial use, please contact me at and Prof. Fu Zhang at to discuss an alternative license. diff --git a/config/HILTI22.yaml b/config/HILTI22.yaml index c8cddbc2..f47a0948 100644 --- a/config/HILTI22.yaml +++ b/config/HILTI22.yaml @@ -3,7 +3,6 @@ common: lid_topic: "/hesai/pandar" imu_topic: "/alphasense/imu" img_en: 1 - lidar_en: 1 ros_driver_bug_fix: false extrin_calib: @@ -49,7 +48,6 @@ vio: inv_expo_cov: 0.1 imu: - imu_en: true imu_int_frame: 30 acc_cov: 0.5 # 0.1 gyr_cov: 0.01 @@ -98,3 +96,6 @@ pcd_save: image_save: img_save_en: false interval: 1 + +lru_config: + lru_size: 1000000 \ No newline at end of file diff --git a/config/MARS_LVIG.yaml b/config/MARS_LVIG.yaml index b0a3b45d..0f24789c 100644 --- a/config/MARS_LVIG.yaml +++ b/config/MARS_LVIG.yaml @@ -3,7 +3,6 @@ common: lid_topic: "/livox/lidar" imu_topic: "/livox/imu" img_en: 1 - lidar_en: 1 ros_driver_bug_fix: false extrin_calib: @@ -65,7 +64,6 @@ vio: inv_expo_cov: 0.1 imu: - imu_en: true imu_int_frame: 30 acc_cov: 2.0 # 0.5 gyr_cov: 0.1 # 0.3 @@ -113,4 +111,7 @@ pcd_save: image_save: img_save_en: false - interval: 1 \ No newline at end of file + interval: 1 + +lru_config: + lru_size: 1000000 \ No newline at end of file diff --git a/config/NTU_VIRAL.yaml b/config/NTU_VIRAL.yaml index 4c1f11b9..5b4d6e3b 100644 --- a/config/NTU_VIRAL.yaml +++ b/config/NTU_VIRAL.yaml @@ -3,7 +3,6 @@ common: lid_topic: "/os1_cloud_node1/points" imu_topic: "/imu/imu" img_en: 1 - lidar_en: 1 ros_driver_bug_fix: false extrin_calib: @@ -41,7 +40,6 @@ vio: inv_expo_cov: 0.1 imu: - imu_en: true imu_int_frame: 30 acc_cov: 0.5 # 0.2 gyr_cov: 0.3 # 0.5 @@ -89,4 +87,7 @@ pcd_save: image_save: img_save_en: false - interval: 1 \ No newline at end of file + interval: 1 + +lru_config: + lru_size: 1000000 \ No newline at end of file diff --git a/config/avia.yaml b/config/avia.yaml index b0fc0b15..a6d0d0f7 100755 --- a/config/avia.yaml +++ b/config/avia.yaml @@ -3,16 +3,15 @@ common: lid_topic: "/livox/lidar" imu_topic: "/livox/imu" img_en: 1 - lidar_en: 1 ros_driver_bug_fix: false extrin_calib: extrinsic_T: [0.04165, 0.02326, -0.0284] extrinsic_R: [1, 0, 0, 0, 1, 0, 0, 0, 1] - Rcl: [0.00610193,-0.999863,-0.0154172, - -0.00615449,0.0153796,-0.999863, - 0.999962,0.00619598,-0.0060598] - Pcl: [0.0194384, 0.104689,-0.0251952] + Rcl: [-0.0036250, -0.9998907, -0.0143360, + 0.0075568, 0.0143083, -0.9998690, + 0.9999649, -0.0037329, 0.0075041] + Pcl: [0.00549469, 0.0712101, 0.0322054] time_offset: imu_time_offset: 0.0 @@ -21,7 +20,7 @@ time_offset: preprocess: point_filter_num: 1 - filter_size_surf: 0.1 + filter_size_surf: 0.1 lidar_type: 1 # Livox Avia LiDAR scan_line: 6 blind: 0.8 @@ -39,7 +38,6 @@ vio: inv_expo_cov: 0.1 imu: - imu_en: true imu_int_frame: 30 acc_cov: 0.5 # 0.2 gyr_cov: 0.3 # 0.5 @@ -88,3 +86,6 @@ pcd_save: image_save: img_save_en: false interval: 1 + +lru_config: + lru_size: 1000000 diff --git a/config/camera_pinhole.yaml b/config/camera_pinhole.yaml index 83fd89e0..8aeb04e8 100644 --- a/config/camera_pinhole.yaml +++ b/config/camera_pinhole.yaml @@ -2,11 +2,11 @@ cam_model: Pinhole cam_width: 1280 cam_height: 1024 scale: 0.5 -cam_fx: 1293.56944 -cam_fy: 1293.3155 -cam_cx: 626.91359 -cam_cy: 522.799224 -cam_d0: -0.076160 -cam_d1: 0.123001 -cam_d2: -0.00113 -cam_d3: 0.000251 \ No newline at end of file +cam_fx: 1311.89517127580 +cam_fy: 1311.36488586115 +cam_cx: 656.523841857393 +cam_cy: 504.136322840350 +cam_d0: -0.0780830982640722 +cam_d1: 0.146382433670493 +cam_d2: -0.00110111633050301 +cam_d3: -0.00110752991013068 \ No newline at end of file diff --git a/include/IMU_Processing.h b/include/IMU_Processing.h index fbc815e9..815c414b 100644 --- a/include/IMU_Processing.h +++ b/include/IMU_Processing.h @@ -18,8 +18,9 @@ which is included as part of this source code package. #include #include #include + #include -const bool time_list(PointType &x, PointType &y) { return (x.curvature < y.curvature); } +const bool time_list(PointXYZIN &x, PointXYZIN &y) { return (x.curvature < y.curvature); } /// *************IMU Process and undistortion class ImuProcess @@ -31,60 +32,56 @@ class ImuProcess ~ImuProcess(); void Reset(); - void Reset(double start_timestamp, const sensor_msgs::ImuConstPtr &lastimu); - void set_extrinsic(const V3D &transl, const M3D &rot); - void set_extrinsic(const V3D &transl); - void set_extrinsic(const MD(4, 4) & T); - void set_gyr_cov_scale(const V3D &scaler); - void set_acc_cov_scale(const V3D &scaler); - void set_gyr_bias_cov(const V3D &b_g); - void set_acc_bias_cov(const V3D &b_a); - void set_inv_expo_cov(const double &inv_expo); - void set_imu_init_frame_num(const int &num); - void disable_imu(); - void disable_gravity_est(); - void disable_bias_est(); - void disable_exposure_est(); - void Process2(LidarMeasureGroup &lidar_meas, StatesGroup &stat, PointCloudXYZI::Ptr cur_pcl_un_); - void UndistortPcl(LidarMeasureGroup &lidar_meas, StatesGroup &state_inout, PointCloudXYZI &pcl_out); + void SetExtrinsic(const V3D &transl, const M3D &rot); + void SetExtrinsic(const V3D &transl); + void SetExtrinsic(const MD(4, 4) & T); + void SetGyrCovScale(const V3D &scaler); + void SetAccCovScale(const V3D &scaler); + void SetGyrBiasCov(const V3D &b_g); + void SetAccBiasCov(const V3D &b_a); + void SetInvExpoCov(const double &inv_expo); + void SetImuInitFrameNum(const int &num); + void DisableGravityEst(); + void DisableBiasEst(); + void DisableExposureEst(); + void Process(LidarMeasureGroup &lidar_meas, StatesGroup &stat, PointCloudXYZIN::Ptr cur_pcl_un_); + void UndistortPcl(LidarMeasureGroup &lidar_meas, StatesGroup &state_inout, PointCloudXYZIN &pcl_out); - ofstream fout_imu; - double IMU_mean_acc_norm; - V3D unbiased_gyr; + std::ofstream fout_imu_; + double imu_mean_acc_norm_; + V3D unbiased_gyr_; - V3D cov_acc; - V3D cov_gyr; - V3D cov_bias_gyr; - V3D cov_bias_acc; - double cov_inv_expo; - double first_lidar_time; - bool imu_time_init = false; - bool imu_need_init = true; - M3D Eye3d; - V3D Zero3d; - int lidar_type; + V3D cov_acc_; + V3D cov_gyr_; + V3D cov_bias_gyr_; + V3D cov_bias_acc_; + double cov_inv_expo_; + double first_lidar_time_; + bool imu_time_init_ = false; + bool imu_need_init_ = true; + M3D eye3d_; + V3D zero3d_; + int lidar_type_; private: - void IMU_init(const MeasureGroup &meas, StatesGroup &state, int &N); - void Forward_without_imu(LidarMeasureGroup &meas, StatesGroup &state_inout, PointCloudXYZI &pcl_out); - PointCloudXYZI pcl_wait_proc; - sensor_msgs::ImuConstPtr last_imu; - PointCloudXYZI::Ptr cur_pcl_un_; - vector IMUpose; - M3D Lid_rot_to_IMU; - V3D Lid_offset_to_IMU; - V3D mean_acc; - V3D mean_gyr; - V3D angvel_last; - V3D acc_s_last; - double last_prop_end_time; - double time_last_scan; - int init_iter_num = 1, MAX_INI_COUNT = 20; - bool b_first_frame = true; - bool imu_en = true; - bool gravity_est_en = true; - bool ba_bg_est_en = true; - bool exposure_estimate_en = true; + void ImuInit(const MeasureGroup &meas, StatesGroup &state, int &N); + PointCloudXYZIN pcl_wait_proc_; + sensor_msgs::ImuConstPtr last_imu_; + PointCloudXYZIN::Ptr cur_pcl_un_; + std::vector imu_pose_; + M3D lidar_rot_to_imu_; + V3D lidar_offset_to_imu_; + V3D mean_acc_; + V3D mean_gyr_; + V3D angvel_last_; + V3D acc_s_last_; + double last_prop_end_time_; + double time_last_scan_; + int init_iter_num_ = 1, max_ini_count_ = 20; + bool b_first_frame_ = true; + bool gravity_est_en_ = true; + bool ba_bg_est_en_ = true; + bool exposure_estimate_en_ = true; }; typedef std::shared_ptr ImuProcessPtr; #endif \ No newline at end of file diff --git a/include/LIVMapper.h b/include/LIVMapper.h index 46664d2b..6ed89e63 100644 --- a/include/LIVMapper.h +++ b/include/LIVMapper.h @@ -1,4 +1,4 @@ -/* +/* This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. Developer: Chunran Zheng @@ -13,175 +13,190 @@ which is included as part of this source code package. #ifndef LIV_MAPPER_H #define LIV_MAPPER_H -#include "IMU_Processing.h" -#include "vio.h" -#include "preprocess.h" #include #include #include -#include +#include + +#include "IMU_Processing.h" +#include "preprocess.h" +#include "vio.h" -class LIVMapper -{ -public: - LIVMapper(ros::NodeHandle &nh); +class LIVMapper { + public: + LIVMapper(ros::NodeHandle &nh, const std::string &camera_config); ~LIVMapper(); - void initializeSubscribersAndPublishers(ros::NodeHandle &nh, image_transport::ImageTransport &it); - void initializeComponents(); - void initializeFiles(); - void run(); - void gravityAlignment(); - void handleFirstFrame(); - void stateEstimationAndMapping(); - void handleVIO(); - void handleLIO(); - void savePCD(); - void processImu(); - - bool sync_packages(LidarMeasureGroup &meas); - void prop_imu_once(StatesGroup &imu_prop_state, const double dt, V3D acc_avr, V3D angvel_avr); - void imu_prop_callback(const ros::TimerEvent &e); - void transformLidar(const Eigen::Matrix3d rot, const Eigen::Vector3d t, const PointCloudXYZI::Ptr &input_cloud, PointCloudXYZI::Ptr &trans_cloud); - void pointBodyToWorld(const PointType &pi, PointType &po); - void RGBpointBodyLidarToIMU(PointType const *const pi, PointType *const po); - void RGBpointBodyToWorld(PointType const *const pi, PointType *const po); - void standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg); - 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 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); - void publish_effect_world(const ros::Publisher &pubLaserCloudEffect, const std::vector &ptpl_list); - void publish_odometry(const ros::Publisher &pubOdomAftMapped); - void publish_mavros(const ros::Publisher &mavros_pose_publisher); - void publish_path(const ros::Publisher pubPath); - void readParameters(ros::NodeHandle &nh); - 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); - - std::mutex mtx_buffer, mtx_buffer_imu_prop; - std::condition_variable sig_buffer; + void InitializeSubscribersAndPublishers(ros::NodeHandle &nh, + image_transport::ImageTransport &it); + void InitializeComponents(); + void InitializeFiles(); + void Run(); + void GravityAlignment(); + void HandleFirstFrame(); + void StateEstimationAndMapping(); + void HandleVIO(); + void HandleLIO(); + void SavePCD(); + void ProcessImu(); + + bool SyncPackages(LidarMeasureGroup &meas); + void PropImuOnce(StatesGroup &imu_prop_state, const double dt, V3D acc_avr, + V3D angvel_avr); + void ImuPropCallback(const ros::TimerEvent &e); + void TransformLidar(const Eigen::Matrix3d rot, const Eigen::Vector3d t, + const PointCloudXYZIN::Ptr &input_cloud, + PointCloudXYZIN::Ptr &trans_cloud); + void PointBodyToWorld(const PointXYZIN &pi, PointXYZIN &po); + void RGBpointBodyLidarToIMU(PointXYZIN const *const pi, PointXYZIN *const po); + void RGBpointBodyToWorld(PointXYZIN const *const pi, PointXYZIN *const po); + void PointCloud2Cbk(const sensor_msgs::PointCloud2::ConstPtr &msg); + void LivoxCbk(const livox_ros_driver::CustomMsg::ConstPtr &msg_in); + void ImuCbk(const sensor_msgs::Imu::ConstPtr &msg_in); + void ImageCbk(const sensor_msgs::ImageConstPtr &msg_in); + void PublishImgRGB(const image_transport::Publisher &pubImage, + VIOManagerPtr vio_manager); + void PublishFrameWorld(const ros::Publisher &pubLaserCloudFullRes, + VIOManagerPtr vio_manager); + void PublishVisualSubMap(const ros::Publisher &pubSubVisualMap); + void PublishEffectWorld(const ros::Publisher &pubLaserCloudEffect, + const std::vector &ptpl_list); + void PublishOdometry(const ros::Publisher &pubOdomAftMapped); + void PublishMavros(const ros::Publisher &mavros_pose_publisher); + void PublishPath(const ros::Publisher pubPath); + void ReadParameters(ros::NodeHandle &nh); + template + void SetPosestamp(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); + + std::mutex mtx_buffer_, mtx_buffer_imu_prop_; + std::condition_variable sig_buffer_; SLAM_MODE slam_mode_; - std::unordered_map voxel_map; - - string root_dir; - string lid_topic, imu_topic, seq_name, img_topic; - V3D extT; - M3D extR; - - int feats_down_size = 0, max_iterations = 0; - - double res_mean_last = 0.05; - double gyr_cov = 0, acc_cov = 0, inv_expo_cov = 0; - double blind_rgb_points = 0.0; - double last_timestamp_lidar = -1.0, last_timestamp_imu = -1.0, last_timestamp_img = -1.0; - double filter_size_surf_min = 0; - double filter_size_pcd = 0; - double _first_lidar_time = 0.0; - double match_time = 0, solve_time = 0, solve_const_H_time = 0; - - bool lidar_map_inited = false, pcd_save_en = false, img_save_en = false, pub_effect_point_en = false, pose_output_en = false, ros_driver_fix_en = false, hilti_en = false; - int img_save_interval = 1, pcd_save_interval = -1, pcd_save_type = 0; - int pub_scan_num = 1; - - StatesGroup imu_propagate, latest_ekf_state; - - bool new_imu = false, state_update_flg = false, imu_prop_enable = true, ekf_finish_once = false; - deque prop_imu_buffer; - sensor_msgs::Imu newest_imu; - double latest_ekf_time; - nav_msgs::Odometry imu_prop_odom; - ros::Publisher pubImuPropOdom; - double imu_time_offset = 0.0; - double lidar_time_offset = 0.0; - - bool gravity_align_en = false, gravity_align_finished = false; - - bool sync_jump_flag = false; - - bool lidar_pushed = false, imu_en, gravity_est_en, flg_reset = false, ba_bg_est_en = true; + + std::string root_dir_; + std::string camera_config_; + std::string lid_topic_, imu_topic_, seq_name_, img_topic_; + V3D ext_t_; + M3D ext_r_; + + int feats_down_size_ = 0, max_iterations_ = 0; + + double res_mean_last_ = 0.05; + double gyr_cov_ = 0, acc_cov_ = 0, inv_expo_cov_ = 0; + double blind_rgb_points_ = 0.0; + double last_timestamp_lidar_ = -1.0, last_timestamp_imu_ = -1.0, + last_timestamp_img_ = -1.0; + double filter_size_surf_min_ = 0; + double filter_size_pcd_ = 0; + double first_lidar_time_ = 0.0; + double match_time_ = 0, solve_time_ = 0, solve_const_H_time_ = 0; + + bool lidar_map_inited_ = false, pcd_save_en_ = false, img_save_en_ = false, + pub_effect_point_en_ = false, pose_output_en_ = false, + ros_driver_fix_en_ = false, hilti_en_ = false; + int img_save_interval_ = 1,pcd_save_interval_ = -1, pcd_save_type_ = 0; + int pub_scan_num_ = 1; + + StatesGroup imu_propagate_, latest_ekf_state_; + + bool new_imu_ = false, state_update_flg_ = false, imu_prop_enable_ = true, + ekf_finish_once_ = false; + std::deque prop_imu_buffer_; + sensor_msgs::Imu newest_imu_; + double latest_ekf_time_; + nav_msgs::Odometry imu_prop_odom_; + ros::Publisher pub_imu_prop_odom_; + double imu_time_offset_ = 0.0; + double lidar_time_offset_ = 0.0; + + bool gravity_align_en_ = false, gravity_align_finished_ = false; + + bool sync_jump_flag_ = false; + + bool lidar_pushed_ = false, gravity_est_en_, flg_reset_ = false, + ba_bg_est_en_ = true; bool dense_map_en = false; - int img_en = 1, imu_int_frame = 3; - bool normal_en = true; - bool exposure_estimate_en = false; - double exposure_time_init = 0.0; - bool inverse_composition_en = false; - bool raycast_en = false; - int lidar_en = 1; - bool is_first_frame = false; - int grid_size, patch_size, grid_n_width, grid_n_height, patch_pyrimid_level; - double outlier_threshold; - double plot_time; - int frame_cnt; - double img_time_offset = 0.0; - deque lid_raw_data_buffer; - deque lid_header_time_buffer; - deque imu_buffer; - deque img_buffer; - deque img_time_buffer; - vector _pv_list; - vector extrinT; - vector extrinR; - vector cameraextrinT; - vector cameraextrinR; - double IMG_POINT_COV; - - PointCloudXYZI::Ptr visual_sub_map; - PointCloudXYZI::Ptr feats_undistort; - PointCloudXYZI::Ptr feats_down_body; - PointCloudXYZI::Ptr feats_down_world; - PointCloudXYZI::Ptr pcl_w_wait_pub; - PointCloudXYZI::Ptr pcl_wait_pub; - PointCloudXYZRGB::Ptr pcl_wait_save; - PointCloudXYZI::Ptr pcl_wait_save_intensity; - - ofstream fout_pre, fout_out, fout_visual_pos, fout_lidar_pos, fout_points; - - pcl::VoxelGrid downSizeFilterSurf; - - V3D euler_cur; - - LidarMeasureGroup LidarMeasures; - StatesGroup _state; - StatesGroup state_propagat; - - nav_msgs::Path path; - nav_msgs::Odometry odomAftMapped; - geometry_msgs::Quaternion geoQuat; - geometry_msgs::PoseStamped msg_body_pose; - - PreprocessPtr p_pre; - ImuProcessPtr p_imu; - VoxelMapManagerPtr voxelmap_manager; - VIOManagerPtr vio_manager; - - ros::Publisher plane_pub; - ros::Publisher voxel_pub; - ros::Subscriber sub_pcl; - ros::Subscriber sub_imu; - ros::Subscriber sub_img; - ros::Publisher pubLaserCloudFullRes; - ros::Publisher pubNormal; - ros::Publisher pubSubVisualMap; - ros::Publisher pubLaserCloudEffect; - ros::Publisher pubLaserCloudMap; - ros::Publisher pubOdomAftMapped; - ros::Publisher pubPath; - ros::Publisher pubLaserCloudDyn; - ros::Publisher pubLaserCloudDynRmed; - ros::Publisher pubLaserCloudDynDbg; - image_transport::Publisher pubImage; - ros::Publisher mavros_pose_publisher; - ros::Timer imu_prop_timer; - - int frame_num = 0; - double aver_time_consu = 0; - double aver_time_icp = 0; - double aver_time_map_inre = 0; - bool colmap_output_en = false; + int img_en_ = 1, imu_int_frame_ = 3; + bool normal_en_ = true; + bool exposure_estimate_en_ = false; + double exposure_time_init_ = 0.0; + bool inverse_composition_en_ = false; + bool raycast_en_ = false; + bool first_frame_finished_ = false; + int grid_size_, patch_size_, patch_pyrimid_level_; + double outlier_threshold_; + double plot_time_; + int frame_cnt_; + double img_time_offset_ = 0.0; + std::deque lid_raw_data_buffer_; + std::deque lid_header_time_buffer_; + std::deque imu_buffer_; + std::deque img_buffer_; + std::deque img_time_buffer_; + std::vector pv_list_; + std::vector extrin_t_; + std::vector extrin_r_; + std::vector cameraextrin_t_; + std::vector cameraextrin_r_; + double img_point_cov_; + + PointCloudXYZIN::Ptr visual_sub_map_; + PointCloudXYZIN::Ptr feats_undistort_; + PointCloudXYZIN::Ptr feats_down_body_; + PointCloudXYZIN::Ptr feats_down_world_; + PointCloudXYZIN::Ptr pcl_w_wait_pub_; + PointCloudXYZIN::Ptr pcl_wait_pub_; + PointCloudXYZRGB::Ptr pcl_wait_save_; + PointCloudXYZIN::Ptr pcl_wait_save_intensity_; + + std::ofstream fout_pre_, fout_out_, fout_visual_pos_, fout_lidar_pos_, fout_points_; + + pcl::VoxelGrid downSize_filter_surf_; + + V3D euler_cur_; + + LidarMeasureGroup lidar_measures_; + StatesGroup state_; + StatesGroup state_propagat_; + + nav_msgs::Path path_; + nav_msgs::Odometry odom_aft_mapped_; + geometry_msgs::Quaternion geo_quat_; + geometry_msgs::PoseStamped msg_body_pose_; + + PreprocessPtr p_pre_; + ImuProcessPtr p_imu_; + VoxelMapManagerPtr voxel_map_manager_; + VIOManagerPtr vio_manager_; + + ros::Publisher plane_pub_; + ros::Publisher voxel_pub_; + ros::Subscriber sub_pcl_; + ros::Subscriber sub_imu_; + ros::Subscriber sub_img_; + ros::Publisher pubLaser_cloud_full_res_; + ros::Publisher pub_normal_; + ros::Publisher pub_sub_visual_map_; + ros::Publisher pub_laser_cloud_effect_; + ros::Publisher pub_laser_cloud_map_; + ros::Publisher pub_odom_aft_mapped_; + ros::Publisher pub_path_; + ros::Publisher pub_laser_cloud_dyn_; + ros::Publisher pub_laser_cloud_dyn_rmed_; + ros::Publisher pub_laser_cloud_dyn_dbg_; + image_transport::Publisher pub_image_; + ros::Publisher mavros_pose_publisher_; + ros::Timer imu_prop_timer_; + + int frame_num_ = 0; + double aver_time_consu_ = 0; + double aver_time_icp_ = 0; + double aver_time_map_inre_ = 0; + bool colmap_output_en_ = false; }; #endif \ No newline at end of file diff --git a/include/common_lib.h b/include/common_lib.h index 9de8a905..3af448e5 100755 --- a/include/common_lib.h +++ b/include/common_lib.h @@ -1,4 +1,4 @@ -/* +/* This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. Developer: Chunran Zheng @@ -13,17 +13,14 @@ which is included as part of this source code package. #ifndef COMMON_LIB_H #define COMMON_LIB_H -#include -#include -#include -#include #include -#include #include +#include +#include +#include +#include -using namespace std; -using namespace Eigen; -using namespace Sophus; +#include #define print_line std::cout << __FILE__ << ", " << __LINE__ << std::endl; #define G_m_s2 (9.81) // Gravaty const in GuangDong/China @@ -33,10 +30,15 @@ using namespace Sophus; #define SIZE_SMALL (100) #define VEC_FROM_ARRAY(v) v[0], v[1], v[2] #define MAT_FROM_ARRAY(v) v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8] -#define DEBUG_FILE_DIR(name) (string(string(ROOT_DIR) + "Log/" + name)) +#define DEBUG_FILE_DIR(name) \ + (std::string(std::string(ROOT_DIR) + "Log/" + name)) -enum LID_TYPE -{ +using SE3 = Sophus::SE3d; +using Mat19d = Eigen::Matrix; // R:0~2, p:3~5, τ:6, v:7~9, + // bg:10~12, ba:13~15, g:16~18 +using Vec19d = Eigen::Matrix; + +enum LID_TYPE { AVIA = 1, VELO16 = 2, OUST64 = 3, @@ -45,72 +47,49 @@ enum LID_TYPE PANDAR128 = 6, ROBOSENSE = 7 }; -enum SLAM_MODE -{ - ONLY_LO = 0, - ONLY_LIO = 1, - LIVO = 2 -}; -enum EKF_STATE -{ - WAIT = 0, - VIO = 1, - LIO = 2, - LO = 3 -}; +enum SLAM_MODE { ONLY_LIO = 0, LIVO = 1}; -struct MeasureGroup -{ +struct MeasureGroup { double vio_time; double lio_time; - deque imu; + std::deque imu; cv::Mat img; - MeasureGroup() - { + MeasureGroup() { vio_time = 0.0; lio_time = 0.0; }; }; -struct LidarMeasureGroup -{ +struct LidarMeasureGroup { double lidar_frame_beg_time; double lidar_frame_end_time; double last_lio_update_time; - PointCloudXYZI::Ptr lidar; - PointCloudXYZI::Ptr pcl_proc_cur; - PointCloudXYZI::Ptr pcl_proc_next; - deque measures; - EKF_STATE lio_vio_flg; - int lidar_scan_index_now; - - LidarMeasureGroup() - { + PointCloudXYZIN::Ptr lidar; + PointCloudXYZIN::Ptr pcl_proc_cur; + PointCloudXYZIN::Ptr pcl_proc_next; + std::deque measures; + + LidarMeasureGroup() { lidar_frame_beg_time = -0.0; lidar_frame_end_time = 0.0; last_lio_update_time = -1.0; - lio_vio_flg = WAIT; - this->lidar.reset(new PointCloudXYZI()); - this->pcl_proc_cur.reset(new PointCloudXYZI()); - this->pcl_proc_next.reset(new PointCloudXYZI()); + this->lidar.reset(new PointCloudXYZIN()); + this->pcl_proc_cur.reset(new PointCloudXYZIN()); + this->pcl_proc_next.reset(new PointCloudXYZIN()); this->measures.clear(); - lidar_scan_index_now = 0; - last_lio_update_time = -1.0; }; }; -typedef struct pointWithVar -{ - Eigen::Vector3d point_b; // point in the lidar body frame - Eigen::Vector3d point_i; // point in the imu body frame - Eigen::Vector3d point_w; // point in the world frame - Eigen::Matrix3d var_nostate; // the var removed the state covarience +typedef struct pointWithVar { + Eigen::Vector3d point_b; // point in the lidar body frame + Eigen::Vector3d point_i; // point in the imu body frame + Eigen::Vector3d point_w; // point in the world frame + Eigen::Matrix3d var_nostate; // the var removed the state covarience Eigen::Matrix3d body_var; Eigen::Matrix3d var; Eigen::Matrix3d point_crossmat; Eigen::Vector3d normal; - pointWithVar() - { + pointWithVar() { var_nostate = Eigen::Matrix3d::Zero(); var = Eigen::Matrix3d::Zero(); body_var = Eigen::Matrix3d::Zero(); @@ -122,11 +101,8 @@ typedef struct pointWithVar }; } pointWithVar; - -struct StatesGroup -{ - StatesGroup() - { +struct StatesGroup { + StatesGroup() { this->rot_end = M3D::Identity(); this->pos_end = V3D::Zero(); this->vel_end = V3D::Zero(); @@ -134,13 +110,12 @@ struct StatesGroup this->bias_a = V3D::Zero(); this->gravity = V3D::Zero(); this->inv_expo_time = 1.0; - this->cov = MD(DIM_STATE, DIM_STATE)::Identity() * INIT_COV; + this->cov = Mat19d::Identity() * INIT_COV; this->cov(6, 6) = 0.00001; this->cov.block<9, 9>(10, 10) = MD(9, 9)::Identity() * 0.00001; }; - StatesGroup(const StatesGroup &b) - { + StatesGroup(const StatesGroup &b) { this->rot_end = b.rot_end; this->pos_end = b.pos_end; this->vel_end = b.vel_end; @@ -151,8 +126,7 @@ struct StatesGroup this->cov = b.cov; }; - StatesGroup &operator=(const StatesGroup &b) - { + StatesGroup &operator=(const StatesGroup &b) { this->rot_end = b.rot_end; this->pos_end = b.pos_end; this->vel_end = b.vel_end; @@ -164,10 +138,10 @@ struct StatesGroup return *this; }; - StatesGroup operator+(const Matrix &state_add) - { + StatesGroup operator+(const Eigen::Matrix &state_add) { StatesGroup a; - a.rot_end = this->rot_end * Exp(state_add(0, 0), state_add(1, 0), state_add(2, 0)); + a.rot_end = + this->rot_end * Exp(state_add(0, 0), state_add(1, 0), state_add(2, 0)); a.pos_end = this->pos_end + state_add.block<3, 1>(3, 0); a.inv_expo_time = this->inv_expo_time + state_add(6, 0); a.vel_end = this->vel_end + state_add.block<3, 1>(7, 0); @@ -179,9 +153,10 @@ struct StatesGroup return a; }; - StatesGroup &operator+=(const Matrix &state_add) - { - this->rot_end = this->rot_end * Exp(state_add(0, 0), state_add(1, 0), state_add(2, 0)); + StatesGroup &operator+=( + const Eigen::Matrix &state_add) { + this->rot_end = + this->rot_end * Exp(state_add(0, 0), state_add(1, 0), state_add(2, 0)); this->pos_end += state_add.block<3, 1>(3, 0); this->inv_expo_time += state_add(6, 0); this->vel_end += state_add.block<3, 1>(7, 0); @@ -191,9 +166,8 @@ struct StatesGroup return *this; }; - Matrix operator-(const StatesGroup &b) - { - Matrix a; + Eigen::Matrix operator-(const StatesGroup &b) { + Eigen::Matrix a; M3D rotd(b.rot_end.transpose() * this->rot_end); a.block<3, 1>(0, 0) = Log(rotd); a.block<3, 1>(3, 0) = this->pos_end - b.pos_end; @@ -205,40 +179,45 @@ struct StatesGroup return a; }; - void resetpose() - { + void resetpose() { this->rot_end = M3D::Identity(); this->pos_end = V3D::Zero(); this->vel_end = V3D::Zero(); } - M3D rot_end; // the estimated attitude (rotation matrix) at the end lidar point - V3D pos_end; // the estimated position at the end lidar point (world frame) - V3D vel_end; // the estimated velocity at the end lidar point (world frame) - double inv_expo_time; // the estimated inverse exposure time (no scale) - V3D bias_g; // gyroscope bias - V3D bias_a; // accelerator bias - V3D gravity; // the estimated gravity acceleration - Matrix cov; // states covariance + M3D rot_end; // 当前帧最后时刻的估计姿态（旋转矩阵） + V3D pos_end; // 当前帧最后时刻的估计位置（世界坐标系） + V3D vel_end; // 当前帧最后时刻的估计速度（世界坐标系） + double inv_expo_time; // 当前帧最后时刻的估计逆曝光时间（相对于第一帧） + V3D bias_g; // 陀螺仪偏置 + V3D bias_a; // 加速度计偏置 + V3D gravity; // 估计的重力加速度 + Eigen::Matrix cov; // 状态协方差矩阵 }; template -auto set_pose6d(const double t, const Matrix &a, const Matrix &g, const Matrix &v, const Matrix &p, - const Matrix &R) -{ +auto set_pose6d(const double t, const Eigen::Matrix &a, + const Eigen::Matrix &g, + const Eigen::Matrix &v, + const Eigen::Matrix &p, + const Eigen::Matrix &R) { Pose6D rot_kp; rot_kp.offset_time = t; - for (int i = 0; i < 3; i++) - { + for (int i = 0; i < 3; i++) { rot_kp.acc[i] = a(i); rot_kp.gyr[i] = g(i); rot_kp.vel[i] = v(i); rot_kp.pos[i] = p(i); - for (int j = 0; j < 3; j++) - rot_kp.rot[i * 3 + j] = R(i, j); + for (int j = 0; j < 3; j++) rot_kp.rot[i * 3 + j] = R(i, j); } // Map(rot_kp.rot, 3,3) = R; - return move(rot_kp); + return std::move(rot_kp); } +inline Sophus::SO3d Mat3ToSO3(const Eigen::Matrix &m) { + /// 对R做归一化，防止sophus里的检查不过 + Eigen::Quaterniond q(m.template cast()); + q.normalize(); + return Sophus::SO3d(q); +} #endif \ No newline at end of file diff --git a/include/feature.h b/include/feature.h deleted file mode 100644 index 08a17f87..00000000 --- a/include/feature.h +++ /dev/null @@ -1,56 +0,0 @@ -/* -This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. - -Developer: Chunran Zheng - -For commercial use, please contact me at or -Prof. Fu Zhang at . - -This file is subject to the terms and conditions outlined in the 'LICENSE' file, -which is included as part of this source code package. -*/ - -#ifndef LIVO_FEATURE_H_ -#define LIVO_FEATURE_H_ - -#include "visual_point.h" - -// A salient image region that is tracked across frames. -struct Feature -{ - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - enum FeatureType - { - CORNER, - EDGELET - }; - int id_; - FeatureType type_; //!< Type can be corner or edgelet. - cv::Mat img_; //!< Image associated with the patch feature - Vector2d px_; //!< Coordinates in pixels on pyramid level 0. - Vector3d f_; //!< Unit-bearing vector of the patch feature. - int level_; //!< Image pyramid level where patch feature was extracted. - VisualPoint *point_; //!< Pointer to 3D point which corresponds to the patch feature. - Vector2d grad_; //!< Dominant gradient direction for edglets, normalized. - SE3 T_f_w_; //!< Pose of the frame where the patch feature was extracted. - float *patch_; //!< Pointer to the image patch data. - float score_; //!< Score of the patch feature. - float mean_; //!< Mean intensity of the image patch feature, used for normalization. - double inv_expo_time_; //!< Inverse exposure time of the image where the patch feature was extracted. - - Feature(VisualPoint *_point, float *_patch, const Vector2d &_px, const Vector3d &_f, const SE3 &_T_f_w, int _level) - : type_(CORNER), px_(_px), f_(_f), T_f_w_(_T_f_w), mean_(0), score_(0), level_(_level), patch_(_patch), point_(_point) - { - } - - inline Vector3d pos() const { return T_f_w_.inverse().translation(); } - - ~Feature() - { - // ROS_WARN("The feature %d has been destructed.", id_); - delete[] patch_; - } -}; - -#endif // LIVO_FEATURE_H_ diff --git a/include/frame.h b/include/frame.h deleted file mode 100644 index 8172dcdc..00000000 --- a/include/frame.h +++ /dev/null @@ -1,84 +0,0 @@ -/* -This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. - -Developer: Chunran Zheng - -For commercial use, please contact me at or -Prof. Fu Zhang at . - -This file is subject to the terms and conditions outlined in the 'LICENSE' file, -which is included as part of this source code package. -*/ - -#ifndef LIVO_FRAME_H_ -#define LIVO_FRAME_H_ - -#include -#include - -class VisualPoint; -struct Feature; - -typedef list Features; -typedef vector ImgPyr; - -/// A frame saves the image, the associated features and the estimated pose. -class Frame : boost::noncopyable -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - static int frame_counter_; //!< Counts the number of created frames. Used to set the unique id. - int id_; //!< Unique id of the frame. - vk::AbstractCamera *cam_; //!< Camera model. - 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. - Features fts_; //!< List of features in the image. - - Frame(vk::AbstractCamera *cam, const cv::Mat &img); - ~Frame(); - - /// Initialize new frame and create image pyramid. - void initFrame(const cv::Mat &img); - - /// Return number of point observations. - inline size_t nObs() const { return fts_.size(); } - - /// Transforms point coordinates in world-frame (w) to camera pixel coordinates (c). - inline Vector2d w2c(const Vector3d &xyz_w) const { return cam_->world2cam(T_f_w_ * xyz_w); } - - /// Transforms point coordinates in world-frame (w) to camera pixel coordinates (c) using the IMU prior pose. - inline Vector2d w2c_prior(const Vector3d &xyz_w) const { return cam_->world2cam(T_f_w_prior_ * xyz_w); } - - /// Transforms pixel coordinates (c) to frame unit sphere coordinates (f). - inline Vector3d c2f(const Vector2d &px) const { return cam_->cam2world(px[0], px[1]); } - - /// Transforms pixel coordinates (c) to frame unit sphere coordinates (f). - inline Vector3d c2f(const double x, const double y) const { return cam_->cam2world(x, y); } - - /// Transforms point coordinates in world-frame (w) to camera-frams (f). - inline Vector3d w2f(const Vector3d &xyz_w) const { return T_f_w_ * xyz_w; } - - /// Transforms point from frame unit sphere (f) frame to world coordinate frame (w). - inline Vector3d f2w(const Vector3d &f) const { return T_f_w_.inverse() * f; } - - /// Projects Point from unit sphere (f) in camera pixels (c). - inline Vector2d f2c(const Vector3d &f) const { return cam_->world2cam(f); } - - /// Return the pose of the frame in the (w)orld coordinate frame. - inline Vector3d pos() const { return T_f_w_.inverse().translation(); } -}; - -typedef std::unique_ptr FramePtr; - -/// Some helper functions for the frame object. -namespace frame_utils -{ - -/// Creates an image pyramid of half-sampled images. -void createImgPyramid(const cv::Mat &img_level_0, int n_levels, ImgPyr &pyr); - -} // namespace frame_utils - -#endif // LIVO_FRAME_H_ diff --git a/include/glogger.h b/include/glogger.h new file mode 100755 index 00000000..7aa3e2c8 --- /dev/null +++ b/include/glogger.h @@ -0,0 +1,66 @@ +#ifndef GLOGGER_H_ +#define GLOGGER_H_ + +#include +#include + +#include +#include + +class GLogger { + public: + // 构造函数 + GLogger(int argc, char** argv, const std::string& log_warning_path, + const std::string& log_path) { + google::InitGoogleLogging(argv[0]); + // 解析输入的命令行参数 + google::ParseCommandLineFlags(&argc, &argv, true); + + // 设置日志路径 + if (log_path != "") { + // 检查并创建信息日志目录 + CheckAndCreateDirectory(log_path, "Log directory"); + google::SetLogDestination(google::INFO, log_path.c_str()); + LOG(WARNING) << "Log path: " << log_path.c_str(); + } + if (log_warning_path != "") { + // 检查并创建警告日志目录 + CheckAndCreateDirectory(log_warning_path, "Log warning directory"); + google::SetLogDestination(google::WARNING, log_warning_path.c_str()); + LOG(WARNING) << "Log warning path: " << log_warning_path.c_str(); + } + + // 安装失败信号处理器，让glog输出失败 + google::InstallFailureSignalHandler(); + FLAGS_alsologtostderr = true; + FLAGS_colorlogtostderr = true; + FLAGS_max_log_size = 50; + } + + // 析构函数 + ~GLogger() { + // 关闭Google日志库 + google::ShutdownGoogleLogging(); + } + + private: + // 检查并创建目录 + void CheckAndCreateDirectory(const std::string& path, + const std::string& log_type) { + std::string dir_path = path; + if (dir_path.back() == '/') { + dir_path.erase(dir_path.end() - 1); + } + if (!std::filesystem::is_directory(dir_path)) { + LOG(ERROR) << log_type << " can not be found: " << dir_path; + if (std::filesystem::create_directories(dir_path)) { + LOG(INFO) << "Create " << log_type << ": " << dir_path; + } else { + LOG(WARNING) << "Failed to create " << log_type << ": " << dir_path; + } + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + } + } +}; + +#endif // GLOGGER_H_ \ No newline at end of file diff --git a/include/image_frame.h b/include/image_frame.h new file mode 100644 index 00000000..3758f1b5 --- /dev/null +++ b/include/image_frame.h @@ -0,0 +1,166 @@ +/* +This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. + +Developer: Chunran Zheng + +For commercial use, please contact me at or +Prof. Fu Zhang at . + +This file is subject to the terms and conditions outlined in the 'LICENSE' file, +which is included as part of this source code package. +*/ + +#ifndef LIVO_FRAME_H_ +#define LIVO_FRAME_H_ + +#include + +#include + +#include "common_lib.h" + +class VisualPoint; +// A salient image region that is tracked across frames. +struct Feature { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + enum FeatureType { CORNER, EDGELET }; + int id_; + FeatureType type_; //!< Type can be corner or edgelet. + cv::Mat img_; //!< Image associated with the patch feature + Eigen::Vector2d px_; //!< Coordinates in pixels on pyramid level 0. + Eigen::Vector3d f_; //!< Unit-bearing vector of the patch feature. + int level_; //!< Image pyramid level where patch feature was extracted. + VisualPoint + *point_; //!< Pointer to 3D point which corresponds to the patch feature. + Eigen::Vector2d + grad_; //!< Dominant gradient direction for edglets, normalized. + SE3 T_f_w_; //!< Pose of the frame where the patch feature was extracted. + float *patch_; //!< Pointer to the image patch data. + float score_; //!< Score of the patch feature. + float mean_; //!< Mean intensity of the image patch feature, used for + //!< normalization. + double inv_expo_time_; //!< Inverse exposure time of the image where the + //!< patch feature was extracted. + + Feature(VisualPoint *_point, float *_patch, const Eigen::Vector2d &_px, + const Eigen::Vector3d &_f, const SE3 &_T_f_w, int _level) + : type_(CORNER), + px_(_px), + f_(_f), + T_f_w_(_T_f_w), + mean_(0), + score_(0), + level_(_level), + patch_(_patch), + point_(_point) {} + + inline Eigen::Vector3d pos() const { return T_f_w_.inverse().translation(); } + + ~Feature() { + // ROS_WARN("The feature %d has been destructed.", id_); + delete[] patch_; + } +}; + +class VisualPoint : boost::noncopyable { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + Eigen::Vector3d pos_; //点的位置 + Eigen::Vector3d normal_; //所在平面法向量 + Eigen::Matrix3d normal_information_; //法向量协方差矩阵的逆 + Eigen::Vector3d previous_normal_; //上次更新的法向量 + std::list obs_; //所有的观察到该点的图像块 + Eigen::Matrix3d covariance_; //点的协方差 + bool is_converged_; //是否收敛 + bool is_normal_initialized_; //法向量是否初始化 + bool has_ref_patch_; //是否存在参考图像块 + Feature *ref_patch; //参考图像块 + + VisualPoint(const Eigen::Vector3d &pos); + ~VisualPoint(); + void findMinScoreFeature(const Eigen::Vector3d &framepos, + Feature *&ftr) const; + void deleteNonRefPatchFeatures(); + void deleteFeatureRef(Feature *ftr); + void addFrameRef(Feature *ftr); + bool getCloseViewObs(const Eigen::Vector3d &pos, Feature *&obs, + const Eigen::Vector2d &cur_px) const; +}; + +typedef std::list Features; +typedef std::vector ImgPyr; +/// A frame saves the image, the associated features and the estimated pose. +class ImageFrame : boost::noncopyable { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + static int frame_counter_; // 帧计数器，用于生成帧的唯一ID + int id_; // 帧的唯一ID + vk::AbstractCamera *cam_; // 相机模型 + SE3 T_f_w_; // 相机在世界系下的位姿 + SE3 T_f_w_prior_; // IMU先验位姿（没有用到） + cv::Mat img_; // 帧的图像 + Features fts_; // 保存特征的链表 + + ImageFrame(vk::AbstractCamera *cam, const cv::Mat &img); + ~ImageFrame(); + + /// 初始化新帧并创建图像金字塔。 + void initFrame(const cv::Mat &img); + + /// 返回点观测的数量。 + inline size_t nObs() const { return fts_.size(); } + + /// 将世界坐标系 (w) 中的点坐标转换为相机像素坐标系 (c) 中的坐标。 + inline Eigen::Vector2d w2c(const Eigen::Vector3d &xyz_w) const { + return cam_->world2cam(T_f_w_ * xyz_w); + } + + /// 使用 IMU 先验姿态将世界坐标系 (w) 中的点坐标转换为相机像素坐标系 (c) + /// 中的坐标。 + inline Eigen::Vector2d w2c_prior(const Eigen::Vector3d &xyz_w) const { + return cam_->world2cam(T_f_w_prior_ * xyz_w); + } + + /// 将相机像素坐标系 (c) 中的坐标转换为帧单位球坐标系 (f) 中的坐标。 + inline Eigen::Vector3d c2f(const Eigen::Vector2d &px) const { + return cam_->cam2world(px[0], px[1]); + } + + /// 将相机像素坐标系 (c) 中的坐标转换为帧单位球坐标系 (f) 中的坐标。 + inline Eigen::Vector3d c2f(const double x, const double y) const { + return cam_->cam2world(x, y); + } + + /// 将世界坐标系 (w) 中的点坐标转换为相机坐标系 (f) 中的坐标。 + inline Eigen::Vector3d w2f(const Eigen::Vector3d &xyz_w) const { + return T_f_w_ * xyz_w; + } + + /// 将帧单位球坐标系 (f) 中的点坐标转换为世界坐标系 (w) 中的坐标。 + inline Eigen::Vector3d f2w(const Eigen::Vector3d &f) const { + return T_f_w_.inverse() * f; + } + + /// 将单位球坐标系 (f) 中的点投影到相机像素坐标系 (c) 中。 + inline Eigen::Vector2d f2c(const Eigen::Vector3d &f) const { + return cam_->world2cam(f); + } + + /// 返回帧在 (w) 世界坐标系中的姿态。 + inline Eigen::Vector3d pos() const { return T_f_w_.inverse().translation(); } +}; + +typedef std::unique_ptr FramePtr; + +/// Some helper functions for the frame object. +namespace frame_utils { + +/// 创建一个由半采样图像组成的图像金字塔。 +void createImgPyramid(const cv::Mat &img_level_0, int n_levels, ImgPyr &pyr); + +} // namespace frame_utils + +#endif // LIVO_FRAME_H_ diff --git a/include/preprocess.h b/include/preprocess.h index bf74f609..4c77cda2 100755 --- a/include/preprocess.h +++ b/include/preprocess.h @@ -1,4 +1,4 @@ -/* +/* This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. Developer: Chunran Zheng @@ -13,16 +13,14 @@ which is included as part of this source code package. #ifndef PREPROCESS_H_ #define PREPROCESS_H_ -#include "common_lib.h" #include #include -using namespace std; +#include "common_lib.h" #define IS_VALID(a) ((abs(a) > 1e8) ? true : false) -enum LiDARFeature -{ +enum LiDARFeature { Nor, Poss_Plane, Real_Plane, @@ -31,30 +29,17 @@ enum LiDARFeature Wire, ZeroPoint }; -enum Surround -{ - Prev, - Next -}; -enum E_jump -{ - Nr_nor, - Nr_zero, - Nr_180, - Nr_inf, - Nr_blind -}; +enum Surround { Prev, Next }; +enum E_jump { Nr_nor, Nr_zero, Nr_180, Nr_inf, Nr_blind }; -struct orgtype -{ +struct orgtype { double range; double dista; double angle[2]; double intersect; E_jump edj[2]; LiDARFeature ftype; - orgtype() - { + orgtype() { range = 0; edj[Prev] = Nr_nor; edj[Next] = Nr_nor; @@ -64,26 +49,24 @@ struct orgtype }; /*** Velodyne ***/ -namespace velodyne_ros -{ -struct EIGEN_ALIGN16 Point -{ +namespace velodyne_ros { +struct EIGEN_ALIGN16 Point { PCL_ADD_POINT4D; float intensity; float time; std::uint16_t ring; EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} // namespace velodyne_ros -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)) +} // namespace velodyne_ros +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)) /****************/ /*** Ouster ***/ -namespace ouster_ros -{ -struct EIGEN_ALIGN16 Point -{ +namespace ouster_ros { +struct EIGEN_ALIGN16 Point { PCL_ADD_POINT4D; float intensity; std::uint32_t t; @@ -93,106 +76,111 @@ struct EIGEN_ALIGN16 Point std::uint32_t range; EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} // namespace ouster_ros -POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point, (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity) - (std::uint32_t, t, t)(std::uint16_t, reflectivity, - reflectivity)(std::uint8_t, ring, ring)(std::uint16_t, ambient, ambient)(std::uint32_t, range, range)) +} // namespace ouster_ros +POINT_CLOUD_REGISTER_POINT_STRUCT( + ouster_ros::Point, + (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)( + std::uint32_t, t, t)(std::uint16_t, reflectivity, reflectivity)( + std::uint8_t, ring, ring)(std::uint16_t, ambient, + ambient)(std::uint32_t, range, range)) /****************/ /*** Hesai_XT32 ***/ -namespace xt32_ros -{ -struct EIGEN_ALIGN16 Point -{ +namespace xt32_ros { +struct EIGEN_ALIGN16 Point { PCL_ADD_POINT4D; float intensity; double timestamp; std::uint16_t ring; EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} // namespace xt32_ros -POINT_CLOUD_REGISTER_POINT_STRUCT(xt32_ros::Point, - (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(double, timestamp, timestamp)(std::uint16_t, ring, ring)) +} // namespace xt32_ros +POINT_CLOUD_REGISTER_POINT_STRUCT( + xt32_ros::Point, + (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)( + double, timestamp, timestamp)(std::uint16_t, ring, ring)) /*****************/ /*** Hesai_Pandar128 ***/ -namespace Pandar128_ros -{ -struct EIGEN_ALIGN16 Point -{ +namespace Pandar128_ros { +struct EIGEN_ALIGN16 Point { PCL_ADD_POINT4D; uint8_t intensity; double timestamp; uint16_t ring; EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} // namespace Pandar128_ros -POINT_CLOUD_REGISTER_POINT_STRUCT(Pandar128_ros::Point, - (float, x, x)(float, y, y)(float, z, z)(std::uint8_t, intensity, intensity)(double, timestamp, timestamp)(std::uint16_t, ring, ring)) +} // namespace Pandar128_ros +POINT_CLOUD_REGISTER_POINT_STRUCT( + Pandar128_ros::Point, + (float, x, x)(float, y, y)(float, z, z)(std::uint8_t, intensity, intensity)( + double, timestamp, timestamp)(std::uint16_t, ring, ring)) /*****************/ /*** Robosense_Airy ***/ -namespace robosense_ros -{ -struct EIGEN_ALIGN16 Point -{ +namespace robosense_ros { +struct EIGEN_ALIGN16 Point { PCL_ADD_POINT4D; float intensity; double timestamp; uint16_t ring; EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} // namespace robosense_ros -POINT_CLOUD_REGISTER_POINT_STRUCT(robosense_ros::Point, - (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(double, timestamp, timestamp)(std::uint16_t, ring, ring)) +} // namespace robosense_ros +POINT_CLOUD_REGISTER_POINT_STRUCT( + robosense_ros::Point, + (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)( + double, timestamp, timestamp)(std::uint16_t, ring, ring)) /*****************/ -class Preprocess -{ -public: +class Preprocess { + public: // EIGEN_MAKE_ALIGNED_OPERATOR_NEW Preprocess(); ~Preprocess(); - void process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out); - void process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out); - void set(bool feat_en, int lid_type, double bld, int pfilt_num); + void Process(const livox_ros_driver::CustomMsg::ConstPtr &msg, + PointCloudXYZIN::Ptr &pcl_out); + void Process(const sensor_msgs::PointCloud2::ConstPtr &msg, + PointCloudXYZIN::Ptr &pcl_out); + void Set(bool feat_en, int lid_type, double bld, int pfilt_num); // sensor_msgs::PointCloud2::ConstPtr pointcloud; - PointCloudXYZI pl_full, pl_corn, pl_surf; - PointCloudXYZI pl_buff[128]; // maximum 128 line lidar - vector typess[128]; // maximum 128 line lidar - int lidar_type, point_filter_num, N_SCANS; - - double blind, blind_sqr; - bool feature_enabled, given_offset_time; - ros::Publisher pub_full, pub_surf, pub_corn; - -private: - 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 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); - void l515_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); - void give_feature(PointCloudXYZI &pl, vector &types); - void pub_func(PointCloudXYZI &pl, const ros::Time &ct); - int plane_judge(const PointCloudXYZI &pl, vector &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct); - bool small_plane(const PointCloudXYZI &pl, vector &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct); - bool edge_jump_judge(const PointCloudXYZI &pl, vector &types, uint i, Surround nor_dir); - - int group_size; - double disA, disB, inf_bound; - double limit_maxmid, limit_midmin, limit_maxmin; - double p2l_ratio; - double jump_up_limit, jump_down_limit; - double cos160; - double edgea, edgeb; - double smallp_intersect, smallp_ratio; - double vx, vy, vz; + PointCloudXYZIN pl_full_, pl_corn_, pl_surf_; + PointCloudXYZIN pl_buff_[128]; // maximum 128 line lidar + std::vector typess_[128]; // maximum 128 line lidar + int lidar_type_, point_filter_num_, n_scans_; + + double blind_, blind_sqr_; + bool feature_enabled_, given_offset_time_; + ros::Publisher pub_full_, pub_surf_, pub_corn_; + + private: + void AviaHandler(const livox_ros_driver::CustomMsg::ConstPtr &msg); + void Oust64Handler(const sensor_msgs::PointCloud2::ConstPtr &msg); + void VelodyneHandler(const sensor_msgs::PointCloud2::ConstPtr &msg); + void Xt32Handler(const sensor_msgs::PointCloud2::ConstPtr &msg); + void Pandar128Handler(const sensor_msgs::PointCloud2::ConstPtr &msg); + void RobosenseHandler(const sensor_msgs::PointCloud2::ConstPtr &msg); + void L515Handler(const sensor_msgs::PointCloud2::ConstPtr &msg); + void GiveFeature(PointCloudXYZIN &pl, std::vector &types); + void PubFunc(PointCloudXYZIN &pl, const ros::Time &ct); + int PlaneJudge(const PointCloudXYZIN &pl, std::vector &types, uint i, + uint &i_nex, Eigen::Vector3d &curr_direct); + bool EdgeJumpJudge(const PointCloudXYZIN &pl, std::vector &types, + uint i, Surround nor_dir); + + int group_size_; + double dis_a_, dis_b_, inf_bound_; + double limit_maxmid_, limit_midmin_, limit_maxmin_; + double p2l_ratio_; + double jump_up_limit_, jump_down_limit_; + double cos160_; + double edge_a_, edge_b_; + double smallp_intersect_, smallp_ratio_; + double vx_, vy_, vz_; }; typedef std::shared_ptr PreprocessPtr; -#endif // PREPROCESS_H_ \ No newline at end of file +#endif // PREPROCESS_H_ \ No newline at end of file diff --git a/include/utils/types.h b/include/utils/types.h index 4e0564a2..31a74173 100644 --- a/include/utils/types.h +++ b/include/utils/types.h @@ -5,11 +5,11 @@ #include #include -typedef pcl::PointXYZINormal PointType; +typedef pcl::PointXYZINormal PointXYZIN; typedef pcl::PointXYZRGB PointTypeRGB; typedef pcl::PointXYZRGBA PointTypeRGBA; -typedef pcl::PointCloud PointCloudXYZI; -typedef std::vector> PointVector; +typedef pcl::PointCloud PointCloudXYZIN; +typedef std::vector> PointVector; typedef pcl::PointCloud PointCloudXYZRGB; typedef pcl::PointCloud PointCloudXYZRGBA; diff --git a/include/vio.h b/include/vio.h index d5600b1d..6c85e9f6 100755 --- a/include/vio.h +++ b/include/vio.h @@ -1,4 +1,4 @@ -/* +/* This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. Developer: Chunran Zheng @@ -13,28 +13,28 @@ which is included as part of this source code package. #ifndef VIO_H_ #define VIO_H_ -#include "voxel_map.h" -#include "feature.h" #include #include -#include #include +#include #include #include -#include -struct SubSparseMap -{ - vector propa_errors; - vector errors; - vector> warp_patch; - vector search_levels; - vector voxel_points; - vector inv_expo_list; - vector add_from_voxel_map; - - SubSparseMap() - { +#include + +#include "image_frame.h" +#include "voxel_map.h" + +struct SubSparseMap { + std::vector propa_errors; + std::vector errors; + std::vector> warp_patch; + std::vector search_levels; + std::vector voxel_points; + std::vector inv_expo_list; + std::vector add_from_voxel_map; + + SubSparseMap() { propa_errors.reserve(SIZE_LARGE); errors.reserve(SIZE_LARGE); warp_patch.reserve(SIZE_LARGE); @@ -44,8 +44,7 @@ struct SubSparseMap add_from_voxel_map.reserve(SIZE_SMALL); }; - void reset() - { + void reset() { propa_errors.clear(); errors.clear(); warp_patch.clear(); @@ -56,132 +55,183 @@ struct SubSparseMap } }; -class Warp -{ -public: - Matrix2d A_cur_ref; +class Warp { + public: + Eigen::Matrix2d A_cur_ref; int search_level; - Warp(int level, Matrix2d warp_matrix) : search_level(level), A_cur_ref(warp_matrix) {} + Warp(int level, Eigen::Matrix2d warp_matrix) + : search_level(level), A_cur_ref(warp_matrix) {} ~Warp() {} }; -class VOXEL_POINTS -{ -public: +class VOXEL_POINTS { + public: std::vector voxel_points; int count; VOXEL_POINTS(int num) : count(num) {} - ~VOXEL_POINTS() - { - for (VisualPoint* vp : voxel_points) - { - if (vp != nullptr) { delete vp; vp = nullptr; } + ~VOXEL_POINTS() { + for (VisualPoint *vp : voxel_points) { + if (vp != nullptr) { + delete vp; + vp = nullptr; + } } } }; - -class VIOManager -{ -public: - int grid_size; - vk::AbstractCamera *cam; - vk::PinholeCamera *pinhole_cam; - StatesGroup *state; - StatesGroup *state_propagat; - M3D Rli, Rci, Rcl, Rcw, Jdphi_dR, Jdp_dt, Jdp_dR; - V3D Pli, Pci, Pcl, Pcw; - vector grid_num; - vector map_index; - vector border_flag; - vector update_flag; - vector map_dist; - vector scan_value; - vector patch_buffer; - bool normal_en, inverse_composition_en, exposure_estimate_en, raycast_en, has_ref_patch_cache; - bool ncc_en = false, colmap_output_en = false; - - int width, height, grid_n_width, grid_n_height, length; - double image_resize_factor; - double fx, fy, cx, cy; - int patch_pyrimid_level, patch_size, patch_size_total, patch_size_half, border, warp_len; - int max_iterations, total_points; - - double img_point_cov, outlier_threshold, ncc_thre; - - SubSparseMap *visual_submap; - std::vector> rays_with_sample_points; - - double compute_jacobian_time, update_ekf_time; - double ave_total = 0; +using VPData = std::pair; + +class VIOManager { + public: + int grid_size_; + vk::AbstractCamera *cam_; + vk::PinholeCamera *pinhole_cam_; + StatesGroup *state_; + StatesGroup *state_propagat_; + M3D Rli_, Rci_, Rcl_, Rcw_, Jdphi_dR_, Jdp_dt_, Jdp_dR_; + V3D Pli_, Pci_, Pcl_, Pcw_; + std::vector grid_num_; + std::vector map_index_; + std::vector border_flag_; + std::vector update_flag_; + std::vector map_dist_; + std::vector scan_value_; + std::vector patch_buffer_; + bool normal_en_, inverse_composition_en_, exposure_estimate_en_, raycast_en_, + has_ref_patch_cache_; + bool ncc_en_ = false, colmap_output_en_ = false; + + int width_, height_, grid_n_width_, grid_n_height_ = 17, length_; + double image_resize_factor_; + double fx_, fy_, cx_, cy_; + int patch_pyrimid_level_, patch_size_, patch_size_total_, patch_size_half_, + border_, warp_len_; + int max_iterations_, total_points_; + + double img_point_cov_, outlier_threshold_, ncc_thre_; + + SubSparseMap *visual_submap_; + std::vector> rays_with_sample_points_; + + double compute_jacobian_time_, update_ekf_time_; + double ave_total_ = 0; // double ave_build_residual_time = 0; // double ave_ekf_time = 0; - int frame_count = 0; - bool plot_flag; + int frame_count_ = 0; + bool plot_flag_; + + Eigen::Matrix G_, H_T_H_; + Eigen::MatrixXd K_, H_sub_inv_; - Matrix G, H_T_H; - MatrixXd K, H_sub_inv; + std::ofstream fout_camera_, fout_colmap_; + // std::unordered_map feat_map_; + std::unordered_map sub_feat_map_; + std::unordered_map warp_map_; - ofstream fout_camera, fout_colmap; - unordered_map feat_map; - unordered_map sub_feat_map; - unordered_map warp_map; - vector retrieve_voxel_points; - vector append_voxel_points; + std::unordered_map::iterator> + vp_map_; + std::list vp_data_; + int lru_size_ = 1000000; + std::vector retrieve_voxel_points_; + std::vector append_voxel_points_; FramePtr new_frame_; - cv::Mat img_cp, img_rgb, img_test; + cv::Mat img_cp_, img_rgb_, img_test_; - enum CellType - { - TYPE_MAP = 1, - TYPE_POINTCLOUD, - TYPE_UNKNOWN - }; + enum CellType { TYPE_MAP = 1, TYPE_POINTCLOUD, TYPE_UNKNOWN }; VIOManager(); + ~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 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); - void setLidarToCameraExtrinsic(vector &R, vector &P); - void initializeVIO(); - void getImagePatch(cv::Mat img, V2D pc, float *patch_tmp, int level); - void computeProjectionJacobian(V3D p, MD(2, 3) & J); - void computeJacobianAndUpdateEKF(cv::Mat img); - void resetGrid(); - void updateVisualMapPoints(cv::Mat img); - void getWarpMatrixAffine(const vk::AbstractCamera &cam, const Vector2d &px_ref, const Vector3d &f_ref, const double depth_ref, const SE3 &T_cur_ref, - const int level_ref, - const int pyramid_level, const int halfpatch_size, Matrix2d &A_cur_ref); - void getWarpMatrixAffineHomography(const vk::AbstractCamera &cam, const V2D &px_ref, - const V3D &xyz_ref, const V3D &normal_ref, const SE3 &T_cur_ref, const int level_ref, Matrix2d &A_cur_ref); - void warpAffine(const Matrix2d &A_cur_ref, const cv::Mat &img_ref, const Vector2d &px_ref, const int level_ref, const int search_level, - const int pyramid_level, const int halfpatch_size, float *patch); - void insertPointIntoVoxelMap(VisualPoint *pt_new); - void plotTrackedPoints(); - void updateFrameState(StatesGroup state); - void projectPatchFromRefToCur(const unordered_map &plane_map); - void updateReferencePatch(const unordered_map &plane_map); - void precomputeReferencePatches(int level); - void dumpDataForColmap(); - double calculateNCC(float *ref_patch, float *cur_patch, int patch_size); - int getBestSearchLevel(const Matrix2d &A_cur_ref, const int max_level); - V3F getInterpolatedPixel(cv::Mat img, V2D pc); - - // void resetRvizDisplay(); - // deque map_cur_frame; - // deque sub_map_ray; - // deque sub_map_ray_fov; - // deque visual_sub_map_cur; - // deque visual_converged_point; - // std::vector> sample_points; - - // PointCloudXYZI::Ptr pg_down; - // pcl::VoxelGrid downSizeFilter; + + void UpdateStateInverse(cv::Mat img, int level); + + void UpdateState(cv::Mat img, int level); + + // void ProcessFrame( + // cv::Mat &img, std::vector &pg, + // const std::unordered_map &feat_map, + // double img_time); + + void ProcessFrame( + cv::Mat &img, std::vector &pv_list, + const std::unordered_map::iterator> &vm_map); + + // void RetrieveFromVisualSparseMap( + // cv::Mat img, std::vector &pg, + // const std::unordered_map &plane_map); + + void RetrieveFromVisualSparseMapLRU( + cv::Mat img, std::vector &pv_list, + const std::unordered_map::iterator> &vm_map); + + void GenerateVisualMapPoints(cv::Mat img, std::vector &pg); + + void SetImuToLidarExtrinsic(const V3D &transl, const M3D &rot); + + void SetLidarToCameraExtrinsic(std::vector &R, + std::vector &P); + + void InitializeVIO(); + + void GetImagePatch(cv::Mat img, V2D pc, float *patch_tmp, int level); + + void ComputeProjectionJacobian(V3D p, MD(2, 3) & J); + + void ComputeJacobianAndUpdateEKF(cv::Mat img); + + void ResetGrid(); + + void UpdateVisualMapPoints(cv::Mat img); + + void GetWarpMatrixAffine(const vk::AbstractCamera &cam, + const Eigen::Vector2d &px_ref, + const Eigen::Vector3d &f_ref, const double depth_ref, + const SE3 &T_cur_ref, const int level_ref, + const int pyramid_level, const int halfpatch_size, + Eigen::Matrix2d &A_cur_ref); + + void GetWarpMatrixAffineHomography(const vk::AbstractCamera &cam, + const V2D &px_ref, const V3D &xyz_ref, + const V3D &normal_ref, + const SE3 &T_cur_ref, const int level_ref, + Eigen::Matrix2d &A_cur_ref); + + void WarpAffine(const Eigen::Matrix2d &A_cur_ref, const cv::Mat &img_ref, + const Eigen::Vector2d &px_ref, const int level_ref, + const int search_level, const int pyramid_level, + const int halfpatch_size, float *patch); + + // void InsertPointIntoFeatureMap(VisualPoint *pt_new); + + void InsertPointIntoFeatureMapLRU(VisualPoint *pt_new); + + void PlotTrackedPoints(); + + void UpdateFrameState(StatesGroup state); + + void ProjectPatchFromRefToCur(); + + void UpdateReferencePatch( + const std::unordered_map &plane_map); + + void UpdateReferencePatch( + const std::unordered_map::iterator> &vm_map); + + void PrecomputeReferencePatches(int level); + + void DumpDataForColmap(); + + double CalculateNCC(float *ref_patch, float *cur_patch, int patch_size); + + int GetBestSearchLevel(const Eigen::Matrix2d &A_cur_ref, const int max_level); + + V3F GetInterpolatedPixel(cv::Mat img, V2D pc); + + SE3 se3_imu_lidar_, se3_lidar_cam_, se3_imu_cam_; }; typedef std::shared_ptr VIOManagerPtr; -#endif // VIO_H_ \ No newline at end of file +#endif // VIO_H_ \ No newline at end of file diff --git a/include/visual_point.h b/include/visual_point.h deleted file mode 100644 index 494cc726..00000000 --- a/include/visual_point.h +++ /dev/null @@ -1,48 +0,0 @@ -/* -This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. - -Developer: Chunran Zheng - -For commercial use, please contact me at or -Prof. Fu Zhang at . - -This file is subject to the terms and conditions outlined in the 'LICENSE' file, -which is included as part of this source code package. -*/ - -#ifndef LIVO_POINT_H_ -#define LIVO_POINT_H_ - -#include -#include "common_lib.h" -#include "frame.h" - -class Feature; - -/// A visual map point on the surface of the scene. -class VisualPoint : boost::noncopyable -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - Vector3d pos_; //!< 3d pos of the point in the world coordinate frame. - Vector3d normal_; //!< Surface normal at point. - Matrix3d normal_information_; //!< Inverse covariance matrix of normal estimation. - Vector3d previous_normal_; //!< Last updated normal vector. - list obs_; //!< Reference patches which observe the point. - Eigen::Matrix3d covariance_; //!< Covariance of the point. - bool is_converged_; //!< True if the point is converged. - bool is_normal_initialized_; //!< True if the normal is initialized. - bool has_ref_patch_; //!< True if the point has a reference patch. - Feature *ref_patch; //!< Reference patch of the point. - - VisualPoint(const Vector3d &pos); - ~VisualPoint(); - void findMinScoreFeature(const Vector3d &framepos, Feature *&ftr) const; - void deleteNonRefPatchFeatures(); - void deleteFeatureRef(Feature *ftr); - void addFrameRef(Feature *ftr); - bool getCloseViewObs(const Vector3d &pos, Feature *&obs, const Vector2d &cur_px) const; -}; - -#endif // LIVO_POINT_H_ diff --git a/include/voxel_map.h b/include/voxel_map.h index 21afaa34..9c7a7d68 100644 --- a/include/voxel_map.h +++ b/include/voxel_map.h @@ -1,4 +1,4 @@ -/* +/* This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. Developer: Chunran Zheng @@ -13,27 +13,28 @@ which is included as part of this source code package. #ifndef VOXEL_MAP_H_ #define VOXEL_MAP_H_ -#include "common_lib.h" -#include -#include #include -#include #include #include #include -#include #include -#include #include #include +#include +#include +#include +#include +#include + +#include "common_lib.h" + #define VOXELMAP_HASH_P 116101 #define VOXELMAP_MAX_N 10000000000 static int voxel_plane_id = 0; -typedef struct VoxelMapConfig -{ +typedef struct VoxelMapConfig { double max_voxel_size_; int max_layer_; int max_iterations_; @@ -51,8 +52,7 @@ typedef struct VoxelMapConfig int half_map_size; } VoxelMapConfig; -typedef struct PointToPlane -{ +typedef struct PointToPlane { Eigen::Vector3d point_b_; Eigen::Vector3d point_w_; Eigen::Vector3d normal_; @@ -66,8 +66,7 @@ typedef struct PointToPlane float dis_to_plane_; } PointToPlane; -typedef struct VoxelPlane -{ +typedef struct VoxelPlane { Eigen::Vector3d center_; Eigen::Vector3d normal_; Eigen::Vector3d y_normal_; @@ -84,8 +83,7 @@ typedef struct VoxelPlane bool is_init_ = false; int id_ = 0; bool is_update_ = false; - VoxelPlane() - { + VoxelPlane() { plane_var_ = Eigen::Matrix::Zero(); covariance_ = Eigen::Matrix3d::Zero(); center_ = Eigen::Vector3d::Zero(); @@ -93,50 +91,51 @@ typedef struct VoxelPlane } } VoxelPlane; -class VOXEL_LOCATION -{ -public: +class VOXEL_LOCATION { + public: int64_t x, y, z; - VOXEL_LOCATION(int64_t vx = 0, int64_t vy = 0, int64_t vz = 0) : x(vx), y(vy), z(vz) {} + VOXEL_LOCATION(int64_t vx = 0, int64_t vy = 0, int64_t vz = 0) + : x(vx), y(vy), z(vz) {} - bool operator==(const VOXEL_LOCATION &other) const { return (x == other.x && y == other.y && z == other.z); } + bool operator==(const VOXEL_LOCATION &other) const { + return (x == other.x && y == other.y && z == other.z); + } }; // Hash value -namespace std -{ -template <> struct hash -{ - int64_t operator()(const VOXEL_LOCATION &s) const - { +namespace std { +template <> +struct hash { + int64_t operator()(const VOXEL_LOCATION &s) const { using std::hash; using std::size_t; - return ((((s.z) * VOXELMAP_HASH_P) % VOXELMAP_MAX_N + (s.y)) * VOXELMAP_HASH_P) % VOXELMAP_MAX_N + (s.x); + return ((((s.z) * VOXELMAP_HASH_P) % VOXELMAP_MAX_N + (s.y)) * + VOXELMAP_HASH_P) % + VOXELMAP_MAX_N + + (s.x); } }; -} // namespace std +} // namespace std -struct DS_POINT -{ +struct DS_POINT { float xyz[3]; float intensity; int count = 0; }; -void calcBodyCov(Eigen::Vector3d &pb, const float range_inc, const float degree_inc, Eigen::Matrix3d &cov); +void CalcBodyCov(Eigen::Vector3d &pb, const float range_inc, + const float degree_inc, Eigen::Matrix3d &cov); -class VoxelOctoTree -{ - -public: +class VoxelOctoTree { + public: VoxelOctoTree() = default; std::vector temp_points_; VoxelPlane *plane_ptr_; int layer_; - int octo_state_; // 0 is end of tree, 1 is not + int octo_state_; // 0 is end of tree, 1 is not VoxelOctoTree *leaves_[8]; - double voxel_center_[3]; // x, y, z + double voxel_center_[3]; // x, y, z std::vector layer_init_num_; float quater_length_; float planer_threshold_; @@ -148,67 +147,75 @@ class VoxelOctoTree bool init_octo_; bool update_enable_; - VoxelOctoTree(int max_layer, int layer, int points_size_threshold, int max_points_num, float planer_threshold) - : max_layer_(max_layer), layer_(layer), points_size_threshold_(points_size_threshold), max_points_num_(max_points_num), - planer_threshold_(planer_threshold) - { + VoxelOctoTree(int max_layer, int layer, int points_size_threshold, + int max_points_num, float planer_threshold) + : max_layer_(max_layer), + layer_(layer), + points_size_threshold_(points_size_threshold), + max_points_num_(max_points_num), + planer_threshold_(planer_threshold) { temp_points_.clear(); octo_state_ = 0; new_points_ = 0; update_size_threshold_ = 5; init_octo_ = false; update_enable_ = true; - for (int i = 0; i < 8; i++) - { + for (int i = 0; i < 8; i++) { leaves_[i] = nullptr; } plane_ptr_ = new VoxelPlane; } - ~VoxelOctoTree() - { - for (int i = 0; i < 8; i++) - { + ~VoxelOctoTree() { + for (int i = 0; i < 8; i++) { delete leaves_[i]; } delete plane_ptr_; } - void init_plane(const std::vector &points, VoxelPlane *plane); - void init_octo_tree(); - void cut_octo_tree(); + + void InitPlane(const std::vector &points, VoxelPlane *plane); + + void InitOctoTree(); + + void CutOctoTree(); + void UpdateOctoTree(const pointWithVar &pv); - VoxelOctoTree *find_correspond(Eigen::Vector3d pw); + VoxelOctoTree *FindCorrespond(Eigen::Vector3d pw); VoxelOctoTree *Insert(const pointWithVar &pv); }; +using VMData = std::pair; void loadVoxelConfig(ros::NodeHandle &nh, VoxelMapConfig &voxel_config); -class VoxelMapManager -{ -public: +class VoxelMapManager { + public: VoxelMapManager() = default; + + VoxelMapManager(VoxelMapConfig &config_setting); + VoxelMapConfig config_setting_; int current_frame_id_ = 0; ros::Publisher voxel_map_pub_; - std::unordered_map voxel_map_; + // std::unordered_map voxel_map_; + std::list vm_data_; + std::unordered_map::iterator> + vm_map_; + int lru_size_ = 1000000; - PointCloudXYZI::Ptr feats_undistort_; - PointCloudXYZI::Ptr feats_down_body_; - PointCloudXYZI::Ptr feats_down_world_; + int undistort_size_ = 0; + PointCloudXYZIN::Ptr feats_down_body_; M3D extR_; V3D extT_; - float build_residual_time, ekf_time; - float ave_build_residual_time = 0.0; - float ave_ekf_time = 0.0; - int scan_count = 0; + float build_residual_time_, ekf_time_; + float ave_build_residual_time_ = 0.0; + float ave_ekf_time_ = 0.0; + int scan_count_ = 0; StatesGroup state_; V3D position_last_; - V3D last_slide_position = {0,0,0}; - - geometry_msgs::Quaternion geoQuat_; + V3D last_slide_position_ = {0, 0, 0}; int feats_down_size_; int effct_feat_num_; @@ -217,43 +224,61 @@ class VoxelMapManager std::vector pv_list_; std::vector ptpl_list_; - VoxelMapManager(VoxelMapConfig &config_setting, std::unordered_map &voxel_map) - : config_setting_(config_setting), voxel_map_(voxel_map) - { - current_frame_id_ = 0; - feats_undistort_.reset(new PointCloudXYZI()); - feats_down_body_.reset(new PointCloudXYZI()); - feats_down_world_.reset(new PointCloudXYZI()); - }; - void StateEstimation(StatesGroup &state_propagat); - void TransformLidar(const Eigen::Matrix3d rot, const Eigen::Vector3d t, const PointCloudXYZI::Ptr &input_cloud, + + void TransformLidar(const Eigen::Matrix3d rot, const Eigen::Vector3d t, + const PointCloudXYZIN::Ptr &input_cloud, pcl::PointCloud::Ptr &trans_cloud); - void BuildVoxelMap(); + // void BuildVoxelMap(); + + void BuildVoxelMapLRU(); + + void BuildVoxelMapLRU(const PointCloudXYZIN::Ptr &cloud_world); + V3F RGBFromVoxel(const V3D &input_point); - void UpdateVoxelMap(const std::vector &input_points); + // void UpdateVoxelMap(const std::vector &input_points); + + void UpdateVoxelMapLRU(const std::vector &input_points); - void BuildResidualListOMP(std::vector &pv_list, std::vector &ptpl_list); + // void BuildResidualListOMP(std::vector &pv_list, + // std::vector &ptpl_list); - void build_single_residual(pointWithVar &pv, const VoxelOctoTree *current_octo, const int current_layer, bool &is_sucess, double &prob, - PointToPlane &single_ptpl); + void BuildResidualListLRU(std::vector &pv_list, + std::vector &ptpl_list); - void pubVoxelMap(); + void BuildSingleResidual(pointWithVar &pv, const VoxelOctoTree *current_octo, + const int current_layer, bool &is_sucess, + double &prob, PointToPlane &single_ptpl); - void mapSliding(); - void clearMemOutOfMap(const int& x_max,const int& x_min,const int& y_max,const int& y_min,const int& z_max,const int& z_min ); + // void PubVoxelMap(); -private: - void GetUpdatePlane(const VoxelOctoTree *current_octo, const int pub_max_voxel_layer, std::vector &plane_list); + void PubVoxelMapLRU(); - void pubSinglePlane(visualization_msgs::MarkerArray &plane_pub, const std::string plane_ns, const VoxelPlane &single_plane, const float alpha, + void MapSliding(); + + void ClearMemOutOfMap(const int &x_max, const int &x_min, const int &y_max, + const int &y_min, const int &z_max, const int &z_min); + + private: + void GetUpdatePlane(const VoxelOctoTree *current_octo, + const int pub_max_voxel_layer, + std::vector &plane_list); + + void PubSinglePlane(visualization_msgs::MarkerArray &plane_pub, + const std::string plane_ns, + const VoxelPlane &single_plane, const float alpha, const Eigen::Vector3d rgb); - void CalcVectQuation(const Eigen::Vector3d &x_vec, const Eigen::Vector3d &y_vec, const Eigen::Vector3d &z_vec, geometry_msgs::Quaternion &q); - void mapJet(double v, double vmin, double vmax, uint8_t &r, uint8_t &g, uint8_t &b); + void CalcVectQuation(const Eigen::Vector3d &x_vec, + const Eigen::Vector3d &y_vec, + const Eigen::Vector3d &z_vec, + geometry_msgs::Quaternion &q); + + void MapJet(double v, double vmin, double vmax, uint8_t &r, uint8_t &g, + uint8_t &b); }; typedef std::shared_ptr VoxelMapManagerPtr; -#endif // VOXEL_MAP_H_ \ No newline at end of file +#endif // VOXEL_MAP_H_ \ No newline at end of file diff --git a/launch/mapping_avia.launch b/launch/mapping_avia.launch index 1e5c3602..d78e51d4 100755 --- a/launch/mapping_avia.launch +++ b/launch/mapping_avia.launch @@ -1,20 +1,27 @@ - + + - + - - + + + + - + - + - - -launch-prefix="gdb -ex run --args" -launch-prefix="valgrind --leak-check=full" - + launch-prefix="gdb -ex run --args" launch-prefix="valgrind + --leak-check=full" \ No newline at end of file diff --git a/launch/mapping_avia_marslvig.launch b/launch/mapping_avia_marslvig.launch index 2fa90d3d..90eb51cf 100755 --- a/launch/mapping_avia_marslvig.launch +++ b/launch/mapping_avia_marslvig.launch @@ -1,20 +1,28 @@ - + + - + - - + + + + - + - + - - -launch-prefix="gdb -ex run --args" -launch-prefix="valgrind --leak-check=full" - + launch-prefix="gdb -ex run --args" launch-prefix="valgrind + --leak-check=full" \ No newline at end of file diff --git a/launch/mapping_hesaixt32_hilti22.launch b/launch/mapping_hesaixt32_hilti22.launch index 59257b02..aa10edb6 100644 --- a/launch/mapping_hesaixt32_hilti22.launch +++ b/launch/mapping_hesaixt32_hilti22.launch @@ -1,14 +1,19 @@ + - - + + + + - + - + - + \ No newline at end of file diff --git a/launch/mapping_ouster_ntu.launch b/launch/mapping_ouster_ntu.launch index a0530d90..f2387776 100755 --- a/launch/mapping_ouster_ntu.launch +++ b/launch/mapping_ouster_ntu.launch @@ -1,15 +1,20 @@ - + + - - + + + + - + - + - + \ No newline at end of file diff --git a/package.xml b/package.xml index c2eedfac..c1e800cb 100755 --- a/package.xml +++ b/package.xml @@ -26,8 +26,6 @@ tf pcl_ros message_generation - vikit_common - vikit_ros cv_bridge geometry_msgs @@ -39,8 +37,6 @@ tf pcl_ros message_runtime - vikit_common - vikit_ros cv_bridge image_transport image_transport diff --git a/src/IMU_Processing.cpp b/src/IMU_Processing.cpp index f3e4c960..d6236f2e 100755 --- a/src/IMU_Processing.cpp +++ b/src/IMU_Processing.cpp @@ -1,4 +1,4 @@ -/* +/* This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. Developer: Chunran Zheng @@ -12,432 +12,276 @@ which is included as part of this source code package. #include "IMU_Processing.h" -ImuProcess::ImuProcess() : Eye3d(M3D::Identity()), - Zero3d(0, 0, 0), b_first_frame(true), imu_need_init(true) -{ - init_iter_num = 1; - cov_acc = V3D(0.1, 0.1, 0.1); - cov_gyr = V3D(0.1, 0.1, 0.1); - cov_bias_gyr = V3D(0.1, 0.1, 0.1); - cov_bias_acc = V3D(0.1, 0.1, 0.1); - cov_inv_expo = 0.2; - mean_acc = V3D(0, 0, -1.0); - mean_gyr = V3D(0, 0, 0); - angvel_last = Zero3d; - acc_s_last = Zero3d; - Lid_offset_to_IMU = Zero3d; - Lid_rot_to_IMU = Eye3d; - last_imu.reset(new sensor_msgs::Imu()); - cur_pcl_un_.reset(new PointCloudXYZI()); +#include + +ImuProcess::ImuProcess() + : eye3d_(M3D::Identity()), + zero3d_(0, 0, 0), + b_first_frame_(true), + imu_need_init_(true) { + init_iter_num_ = 1; + cov_acc_ = V3D(0.1, 0.1, 0.1); + cov_gyr_ = V3D(0.1, 0.1, 0.1); + cov_bias_gyr_ = V3D(0.1, 0.1, 0.1); + cov_bias_acc_ = V3D(0.1, 0.1, 0.1); + cov_inv_expo_ = 0.2; + mean_acc_ = V3D(0, 0, -1.0); + mean_gyr_ = V3D(0, 0, 0); + angvel_last_ = zero3d_; + acc_s_last_ = zero3d_; + lidar_offset_to_imu_ = zero3d_; + lidar_rot_to_imu_ = eye3d_; + last_imu_.reset(new sensor_msgs::Imu()); + cur_pcl_un_.reset(new PointCloudXYZIN()); } ImuProcess::~ImuProcess() {} -void ImuProcess::Reset() -{ +void ImuProcess::Reset() { ROS_WARN("Reset ImuProcess"); - mean_acc = V3D(0, 0, -1.0); - mean_gyr = V3D(0, 0, 0); - angvel_last = Zero3d; - imu_need_init = true; - init_iter_num = 1; - IMUpose.clear(); - last_imu.reset(new sensor_msgs::Imu()); - cur_pcl_un_.reset(new PointCloudXYZI()); + mean_acc_ = V3D(0, 0, -1.0); + mean_gyr_ = V3D(0, 0, 0); + angvel_last_ = zero3d_; + imu_need_init_ = true; + init_iter_num_ = 1; + imu_pose_.clear(); + last_imu_.reset(new sensor_msgs::Imu()); + cur_pcl_un_.reset(new PointCloudXYZIN()); } -void ImuProcess::disable_imu() -{ - cout << "IMU Disabled !!!!!" << endl; - imu_en = false; - imu_need_init = false; +void ImuProcess::DisableGravityEst() { + std::cout << "Online Gravity Estimation Disabled !!!!!" << std::endl; + gravity_est_en_ = false; } -void ImuProcess::disable_gravity_est() -{ - cout << "Online Gravity Estimation Disabled !!!!!" << endl; - gravity_est_en = false; +void ImuProcess::DisableBiasEst() { + std::cout << "Bias Estimation Disabled !!!!!" << std::endl; + ba_bg_est_en_ = false; } -void ImuProcess::disable_bias_est() -{ - cout << "Bias Estimation Disabled !!!!!" << endl; - ba_bg_est_en = false; +void ImuProcess::DisableExposureEst() { + std::cout << "Online Time Offset Estimation Disabled !!!!!" << std::endl; + exposure_estimate_en_ = false; } -void ImuProcess::disable_exposure_est() -{ - cout << "Online Time Offset Estimation Disabled !!!!!" << endl; - exposure_estimate_en = false; +void ImuProcess::SetExtrinsic(const MD(4, 4) & T) { + lidar_offset_to_imu_ = T.block<3, 1>(0, 3); + lidar_rot_to_imu_ = T.block<3, 3>(0, 0); } -void ImuProcess::set_extrinsic(const MD(4, 4) & T) -{ - Lid_offset_to_IMU = T.block<3, 1>(0, 3); - Lid_rot_to_IMU = T.block<3, 3>(0, 0); +void ImuProcess::SetExtrinsic(const V3D &transl) { + lidar_offset_to_imu_ = transl; + lidar_rot_to_imu_.setIdentity(); } -void ImuProcess::set_extrinsic(const V3D &transl) -{ - Lid_offset_to_IMU = transl; - Lid_rot_to_IMU.setIdentity(); +void ImuProcess::SetExtrinsic(const V3D &transl, const M3D &rot) { + lidar_offset_to_imu_ = transl; + lidar_rot_to_imu_ = rot; } -void ImuProcess::set_extrinsic(const V3D &transl, const M3D &rot) -{ - Lid_offset_to_IMU = transl; - Lid_rot_to_IMU = rot; -} - -void ImuProcess::set_gyr_cov_scale(const V3D &scaler) { cov_gyr = scaler; } +void ImuProcess::SetGyrCovScale(const V3D &scaler) { cov_gyr_ = scaler; } -void ImuProcess::set_acc_cov_scale(const V3D &scaler) { cov_acc = scaler; } +void ImuProcess::SetAccCovScale(const V3D &scaler) { cov_acc_ = scaler; } -void ImuProcess::set_gyr_bias_cov(const V3D &b_g) { cov_bias_gyr = b_g; } +void ImuProcess::SetGyrBiasCov(const V3D &b_g) { cov_bias_gyr_ = b_g; } -void ImuProcess::set_inv_expo_cov(const double &inv_expo) { cov_inv_expo = inv_expo; } +void ImuProcess::SetInvExpoCov(const double &inv_expo) { + cov_inv_expo_ = inv_expo; +} -void ImuProcess::set_acc_bias_cov(const V3D &b_a) { cov_bias_acc = b_a; } +void ImuProcess::SetAccBiasCov(const V3D &b_a) { cov_bias_acc_ = b_a; } -void ImuProcess::set_imu_init_frame_num(const int &num) { MAX_INI_COUNT = num; } +void ImuProcess::SetImuInitFrameNum(const int &num) { + max_ini_count_ = num; +} -void ImuProcess::IMU_init(const MeasureGroup &meas, StatesGroup &state_inout, int &N) -{ +void ImuProcess::ImuInit(const MeasureGroup &meas, StatesGroup &state_inout, + int &N) { /** 1. initializing the gravity, gyro bias, acc and gyro covariance ** 2. normalize the acceleration measurenments to unit gravity **/ - ROS_INFO("IMU Initializing: %.1f %%", double(N) / MAX_INI_COUNT * 100); + ROS_INFO("IMU Initializing: %.1f %%", double(N) / max_ini_count_ * 100); V3D cur_acc, cur_gyr; - if (b_first_frame) - { + if (b_first_frame_) { Reset(); N = 1; - b_first_frame = false; + b_first_frame_ = false; const auto &imu_acc = meas.imu.front()->linear_acceleration; const auto &gyr_acc = meas.imu.front()->angular_velocity; - mean_acc << imu_acc.x, imu_acc.y, imu_acc.z; - mean_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z; - // first_lidar_time = meas.lidar_frame_beg_time; - // cout<<"init acc norm: "<linear_acceleration; const auto &gyr_acc = imu->angular_velocity; cur_acc << imu_acc.x, imu_acc.y, imu_acc.z; cur_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z; - mean_acc += (cur_acc - mean_acc) / N; - mean_gyr += (cur_gyr - mean_gyr) / N; + mean_acc_ += (cur_acc - mean_acc_) / N; + mean_gyr_ += (cur_gyr - mean_gyr_) / N; // cov_acc = cov_acc * (N - 1.0) / N + (cur_acc - // mean_acc).cwiseProduct(cur_acc - mean_acc) * (N - 1.0) / (N * N); cov_gyr // = cov_gyr * (N - 1.0) / N + (cur_gyr - mean_gyr).cwiseProduct(cur_gyr - // mean_gyr) * (N - 1.0) / (N * N); - // cout<<"acc norm: "<points.size(); i++) { - // if (dt < pcl_out->points[i].curvature) { - // dt = pcl_out->points[i].curvature; - // } - // } - // dt = dt / (double)1000; - // std::cout << "dt:" << dt << std::endl; - // double dt = pcl_out->points.back().curvature / double(1000); - - /* covariance propagation */ - // M3D acc_avr_skew; - M3D Exp_f = Exp(state_inout.bias_g, dt); - - F_x.setIdentity(); - cov_w.setZero(); - - F_x.block<3, 3>(0, 0) = Exp(state_inout.bias_g, -dt); - F_x.block<3, 3>(0, 10) = Eye3d * dt; - F_x.block<3, 3>(3, 7) = Eye3d * dt; - // F_x.block<3, 3>(6, 0) = - R_imu * acc_avr_skew * dt; - // F_x.block<3, 3>(6, 12) = - R_imu * dt; - // F_x.block<3, 3>(6, 15) = Eye3d * dt; - - cov_w.block<3, 3>(10, 10).diagonal() = cov_gyr * dt * dt; // for omega in constant model - cov_w.block<3, 3>(7, 7).diagonal() = cov_acc * dt * dt; // for velocity in constant model - // cov_w.block<3, 3>(6, 6) = - // R_imu * cov_acc.asDiagonal() * R_imu.transpose() * dt * dt; - // cov_w.block<3, 3>(9, 9).diagonal() = - // cov_bias_gyr * dt * dt; // bias gyro covariance - // cov_w.block<3, 3>(12, 12).diagonal() = - // cov_bias_acc * dt * dt; // bias acc covariance - - // std::cout << "before propagete:" << state_inout.cov.diagonal().transpose() - // << std::endl; - state_inout.cov = F_x * state_inout.cov * F_x.transpose() + cov_w; - // std::cout << "cov_w:" << cov_w.diagonal().transpose() << std::endl; - // std::cout << "after propagete:" << state_inout.cov.diagonal().transpose() - // << std::endl; - state_inout.rot_end = state_inout.rot_end * Exp_f; - state_inout.pos_end = state_inout.pos_end + state_inout.vel_end * dt; - - if (lidar_type != L515) - { - auto it_pcl = pcl_out.points.end() - 1; - double dt_j = 0.0; - for(; it_pcl != pcl_out.points.begin(); it_pcl--) - { - dt_j= pcl_end_offset_time - it_pcl->curvature/double(1000); - M3D R_jk(Exp(state_inout.bias_g, - dt_j)); - V3D P_j(it_pcl->x, it_pcl->y, it_pcl->z); - // Using rotation and translation to un-distort points - V3D p_jk; - p_jk = - state_inout.rot_end.transpose() * state_inout.vel_end * dt_j; - - V3D P_compensate = R_jk * P_j + p_jk; - - /// save Undistorted points and their rotation - it_pcl->x = P_compensate(0); - it_pcl->y = P_compensate(1); - it_pcl->z = P_compensate(2); - } - } -} - - -void ImuProcess::UndistortPcl(LidarMeasureGroup &lidar_meas, StatesGroup &state_inout, PointCloudXYZI &pcl_out) -{ - double t0 = omp_get_wtime(); +void ImuProcess::UndistortPcl(LidarMeasureGroup &lidar_meas, + StatesGroup &state_inout, + PointCloudXYZIN &pcl_out) { pcl_out.clear(); /*** add the imu of the last frame-tail to the of current frame-head ***/ MeasureGroup &meas = lidar_meas.measures.back(); // cout<<"meas.imu.size: "<header.stamp.toSec(); const double &imu_end_time = v_imu.back()->header.stamp.toSec(); - const double prop_beg_time = last_prop_end_time; - // printf("[ IMU ] undistort input size: %zu \n", lidar_meas.pcl_proc_cur->points.size()); - // printf("[ IMU ] IMU data sequence size: %zu \n", meas.imu.size()); - // printf("[ IMU ] lidar_scan_index_now: %d \n", lidar_meas.lidar_scan_index_now); + const double prop_beg_time = last_prop_end_time_; - const double prop_end_time = lidar_meas.lio_vio_flg == LIO ? meas.lio_time : meas.vio_time; + const double prop_end_time = meas.lio_time; /*** cut lidar point based on the propagation-start time and required * propagation-end time ***/ - // const double pcl_offset_time = (prop_end_time - - // lidar_meas.lidar_frame_beg_time) * 1000.; // the offset time w.r.t scan - // start time auto pcl_it = lidar_meas.pcl_proc_cur->points.begin() + - // lidar_meas.lidar_scan_index_now; auto pcl_it_end = - // lidar_meas.lidar->points.end(); printf("[ IMU ] pcl_it->curvature: %lf - // pcl_offset_time: %lf \n", pcl_it->curvature, pcl_offset_time); while - // (pcl_it != pcl_it_end && pcl_it->curvature <= pcl_offset_time) - // { - // pcl_wait_proc.push_back(*pcl_it); - // pcl_it++; - // lidar_meas.lidar_scan_index_now++; - // } - - // cout<<"pcl_out.size(): "<curvature: - // "<curvature<points.size()); - pcl_wait_proc = *(lidar_meas.pcl_proc_cur); - lidar_meas.lidar_scan_index_now = 0; - IMUpose.push_back(set_pose6d(0.0, acc_s_last, angvel_last, state_inout.vel_end, state_inout.pos_end, state_inout.rot_end)); - } - - // printf("[ IMU ] pcl_wait_proc size: %zu \n", pcl_wait_proc.points.size()); - - // sort(pcl_out.points.begin(), pcl_out.points.end(), time_list); - // lidar_meas.debug_show(); - // cout<<"UndistortPcl [ IMU ]: Process lidar from "<points.size()<points.size()); + pcl_wait_proc_ = *(lidar_meas.pcl_proc_cur); + imu_pose_.push_back(set_pose6d(0.0, acc_s_last_, angvel_last_, + state_inout.vel_end, state_inout.pos_end, + state_inout.rot_end)); /*** Initialize IMU pose ***/ - // IMUpose.clear(); - /*** forward propagation at each imu point ***/ - V3D acc_imu(acc_s_last), angvel_avr(angvel_last), acc_avr, vel_imu(state_inout.vel_end), pos_imu(state_inout.pos_end); - // cout << "[ IMU ] input state: " << state_inout.vel_end.transpose() << " " << state_inout.pos_end.transpose() << endl; + // 上一帧最后时刻的状态 + V3D acc_imu(acc_s_last_), angvel_avr(angvel_last_), acc_avr, + vel_imu(state_inout.vel_end), pos_imu(state_inout.pos_end); + M3D R_imu(state_inout.rot_end); - MD(DIM_STATE, DIM_STATE) F_x, cov_w; + Mat19d F_x, cov_w; double dt, dt_all = 0.0; double offs_t; - // double imu_time; + double tau; - if (!imu_time_init) - { - // imu_time = v_imu.front()->header.stamp.toSec() - first_lidar_time; - // tau = 1.0 / (0.25 * sin(2 * CV_PI * 0.5 * imu_time) + 0.75); + if (!imu_time_init_) { tau = 1.0; - imu_time_init = true; - } - else - { + imu_time_init_ = true; + } else { tau = state_inout.inv_expo_time; - // ROS_ERROR("tau: %.6f !!!!!!", tau); } - // state_inout.cov(6, 6) = 0.01; - - // ROS_ERROR("lidar_meas.lio_vio_flg"); - // cout<<"lidar_meas.lio_vio_flg: "<header.stamp.toSec() < prop_beg_time) continue; - - angvel_avr << 0.5 * (head->angular_velocity.x + tail->angular_velocity.x), 0.5 * (head->angular_velocity.y + tail->angular_velocity.y), - 0.5 * (head->angular_velocity.z + tail->angular_velocity.z); - - // angvel_avr<angular_velocity.x, tail->angular_velocity.y, - // tail->angular_velocity.z; - - acc_avr << 0.5 * (head->linear_acceleration.x + tail->linear_acceleration.x), 0.5 * (head->linear_acceleration.y + tail->linear_acceleration.y), - 0.5 * (head->linear_acceleration.z + tail->linear_acceleration.z); - - // cout<<"angvel_avr: "<header.stamp.toSec() - first_lidar_time << " " << angvel_avr.transpose() << " " << acc_avr.transpose() << endl; - // #endif - - // imu_time = head->header.stamp.toSec() - first_lidar_time; - - angvel_avr -= state_inout.bias_g; - acc_avr = acc_avr * G_m_s2 / mean_acc.norm() - state_inout.bias_a; - - if (head->header.stamp.toSec() < prop_beg_time) - { - // printf("00 \n"); - dt = tail->header.stamp.toSec() - last_prop_end_time; - offs_t = tail->header.stamp.toSec() - prop_beg_time; - } - else if (i != v_imu.size() - 2) - { - // printf("11 \n"); - dt = tail->header.stamp.toSec() - head->header.stamp.toSec(); - offs_t = tail->header.stamp.toSec() - prop_beg_time; - } - else - { - // printf("22 \n"); - dt = prop_end_time - head->header.stamp.toSec(); - offs_t = prop_end_time - prop_beg_time; - } - dt_all += dt; - // printf("[ LIO Propagation ] dt: %lf \n", dt); - - /* covariance propagation */ - M3D acc_avr_skew; - M3D Exp_f = Exp(angvel_avr, dt); - acc_avr_skew << SKEW_SYM_MATRX(acc_avr); - - F_x.setIdentity(); - cov_w.setZero(); + // IMU位姿预测 + + dt = 0; + for (int i = 0; i < v_imu.size() - 1; i++) { + auto head = v_imu[i]; + auto tail = v_imu[i + 1]; + + if (tail->header.stamp.toSec() < prop_beg_time) continue; + // 中值滤波 + angvel_avr << 0.5 * (head->angular_velocity.x + tail->angular_velocity.x), + 0.5 * (head->angular_velocity.y + tail->angular_velocity.y), + 0.5 * (head->angular_velocity.z + tail->angular_velocity.z); + + acc_avr << 0.5 * + (head->linear_acceleration.x + tail->linear_acceleration.x), + 0.5 * (head->linear_acceleration.y + tail->linear_acceleration.y), + 0.5 * (head->linear_acceleration.z + tail->linear_acceleration.z); + + fout_imu_ << std::setw(10) << head->header.stamp.toSec() - first_lidar_time_ + << " " << angvel_avr.transpose() << " " << acc_avr.transpose() + << std::endl; + + angvel_avr -= state_inout.bias_g; + acc_avr = acc_avr * G_m_s2 / mean_acc_.norm() - state_inout.bias_a; + + if (head->header.stamp.toSec() < prop_beg_time) { + dt = tail->header.stamp.toSec() - last_prop_end_time_; + offs_t = tail->header.stamp.toSec() - prop_beg_time; + } else if (i != v_imu.size() - 2) { + dt = tail->header.stamp.toSec() - head->header.stamp.toSec(); + offs_t = tail->header.stamp.toSec() - prop_beg_time; + } else { + dt = prop_end_time - head->header.stamp.toSec(); + offs_t = prop_end_time - prop_beg_time; + } - F_x.block<3, 3>(0, 0) = Exp(angvel_avr, -dt); - if (ba_bg_est_en) F_x.block<3, 3>(0, 10) = -Eye3d * dt; - // F_x.block<3,3>(3,0) = R_imu * off_vel_skew * dt; - F_x.block<3, 3>(3, 7) = Eye3d * dt; - F_x.block<3, 3>(7, 0) = -R_imu * acc_avr_skew * dt; - if (ba_bg_est_en) F_x.block<3, 3>(7, 13) = -R_imu * dt; - if (gravity_est_en) F_x.block<3, 3>(7, 16) = Eye3d * dt; + dt_all += dt; - // tau = 1.0 / (0.25 * sin(2 * CV_PI * 0.5 * imu_time) + 0.75); - // F_x(6,6) = 0.25 * 2 * CV_PI * 0.5 * cos(2 * CV_PI * 0.5 * imu_time) * (-tau*tau); F_x(18,18) = 0.00001; - if (exposure_estimate_en) cov_w(6, 6) = cov_inv_expo * dt * dt; - cov_w.block<3, 3>(0, 0).diagonal() = cov_gyr * dt * dt; - cov_w.block<3, 3>(7, 7) = R_imu * cov_acc.asDiagonal() * R_imu.transpose() * dt * dt; - cov_w.block<3, 3>(10, 10).diagonal() = cov_bias_gyr * dt * dt; // bias gyro covariance - cov_w.block<3, 3>(13, 13).diagonal() = cov_bias_acc * dt * dt; // bias acc covariance + // 状态预测 + M3D acc_avr_skew; + M3D Exp_f = Exp(angvel_avr, dt); + acc_avr_skew << SKEW_SYM_MATRX(acc_avr); - state_inout.cov = F_x * state_inout.cov * F_x.transpose() + cov_w; - // state_inout.cov.block<18,18>(0,0) = F_x.block<18,18>(0,0) * - // state_inout.cov.block<18,18>(0,0) * F_x.block<18,18>(0,0).transpose() + - // cov_w.block<18,18>(0,0); + F_x.setIdentity(); //雅可比矩阵 + cov_w.setZero(); //噪声 - // tau = tau + 0.25 * 2 * CV_PI * 0.5 * cos(2 * CV_PI * 0.5 * imu_time) * - // (-tau*tau) * dt; + //雅可比矩阵规则 R:0~2, p:3~5, τ:6, v:7~9, bg:10~12, ba:13~15, g:16~18 + F_x.block<3, 3>(0, 0) = Exp(angvel_avr, -dt); // R对R + if (ba_bg_est_en_) F_x.block<3, 3>(0, 10) = -eye3d_ * dt; // R对bg + // F_x.block<3,3>(3,0) = R_imu * off_vel_skew * dt; //p对R + F_x.block<3, 3>(3, 7) = eye3d_ * dt; // p对v + F_x.block<3, 3>(7, 0) = -R_imu * acc_avr_skew * dt; // v对R + if (ba_bg_est_en_) F_x.block<3, 3>(7, 13) = -R_imu * dt; // v对ba + if (gravity_est_en_) F_x.block<3, 3>(7, 16) = eye3d_ * dt; // v对g - // tau = 1.0 / (0.25 * sin(2 * CV_PI * 0.5 * imu_time) + 0.75); + // tau = 1.0 / (0.25 * sin(2 * CV_PI * 0.5 * imu_time) + 0.75); + // F_x(6,6) = 0.25 * 2 * CV_PI * 0.5 * cos(2 * CV_PI * 0.5 * imu_time) * + // (-tau*tau); F_x(18,18) = 0.00001; + if (exposure_estimate_en_) cov_w(6, 6) = cov_inv_expo_ * dt * dt; + cov_w.block<3, 3>(0, 0).diagonal() = cov_gyr_ * dt * dt; + cov_w.block<3, 3>(7, 7) = + R_imu * cov_acc_.asDiagonal() * R_imu.transpose() * dt * dt; + cov_w.block<3, 3>(10, 10).diagonal() = + cov_bias_gyr_ * dt * dt; // bias gyro covariance + cov_w.block<3, 3>(13, 13).diagonal() = + cov_bias_acc_ * dt * dt; // bias acc covariance + + state_inout.cov = F_x * state_inout.cov * F_x.transpose() + cov_w; + // state_inout.cov.block<18,18>(0,0) = F_x.block<18,18>(0,0) * + // state_inout.cov.block<18,18>(0,0) * F_x.block<18,18>(0,0).transpose() + // + cov_w.block<18,18>(0,0); + + // tau = tau + 0.25 * 2 * CV_PI * 0.5 * cos(2 * CV_PI * 0.5 * imu_time) + // * + // (-tau*tau) * dt; - /* propogation of IMU attitude */ - R_imu = R_imu * Exp_f; + // tau = 1.0 / (0.25 * sin(2 * CV_PI * 0.5 * imu_time) + 0.75); - /* Specific acceleration (global frame) of IMU */ - acc_imu = R_imu * acc_avr + state_inout.gravity; + /* propogation of IMU attitude */ + R_imu = R_imu * Exp_f; - /* propogation of IMU */ - pos_imu = pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt; + /* Specific acceleration (global frame) of IMU */ + acc_imu = R_imu * acc_avr + state_inout.gravity; - /* velocity of IMU */ - vel_imu = vel_imu + acc_imu * dt; + /* propogation of IMU */ + pos_imu = pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt; - /* save the poses at each IMU measurements */ - angvel_last = angvel_avr; - acc_s_last = acc_imu; + /* velocity of IMU */ + vel_imu = vel_imu + acc_imu * dt; - // cout<header.stamp.toSec(): - // "<header.stamp.toSec()<header.stamp.toSec(): + // "<header.stamp.toSec()<offset_time<<" "; - // } - // cout<points.size()<rot); acc_imu << VEC_FROM_ARRAY(head->acc); - // cout<<"head imu acc: "<vel); pos_imu << VEC_FROM_ARRAY(head->pos); angvel_avr << VEC_FROM_ARRAY(head->gyr); - // printf("head->offset_time: %lf \n", head->offset_time); - // printf("it_pcl->curvature: %lf pt dt: %lf \n", it_pcl->curvature, - // it_pcl->curvature / double(1000) - head->offset_time); - - for (; it_pcl->curvature / double(1000) > head->offset_time; it_pcl--) - { + for (; it_pcl->curvature / double(1000) > head->offset_time; it_pcl--) { dt = it_pcl->curvature / double(1000) - head->offset_time; /* Transform to the 'end' frame */ M3D R_i(R_imu * Exp(angvel_avr, dt)); - V3D T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt - state_inout.pos_end); + V3D T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt - + state_inout.pos_end); V3D P_i(it_pcl->x, it_pcl->y, it_pcl->z); // V3D P_compensate = Lid_rot_to_IMU.transpose() * // (state_inout.rot_end.transpose() * (R_i * (Lid_rot_to_IMU * P_i + // Lid_offset_to_IMU) + T_ei) - Lid_offset_to_IMU); - V3D P_compensate = (extR_Ri * (R_i * (Lid_rot_to_IMU * P_i + Lid_offset_to_IMU) + T_ei) - exrR_extT); + V3D P_compensate = + (extR_Ri * (R_i * (lidar_rot_to_imu_ * P_i + lidar_offset_to_imu_) + + T_ei) - + exrR_extT); /// save Undistorted points and their rotation it_pcl->x = P_compensate(0); it_pcl->y = P_compensate(1); it_pcl->z = P_compensate(2); - if (it_pcl == pcl_wait_proc.points.begin()) break; + if (it_pcl == pcl_wait_proc_.points.begin()) break; } } - pcl_out = pcl_wait_proc; - pcl_wait_proc.clear(); - IMUpose.clear(); - } - // printf("[ IMU ] time forward: %lf, backward: %lf.\n", t1 - t0, omp_get_wtime() - t1); + pcl_out = pcl_wait_proc_; + pcl_wait_proc_.clear(); + imu_pose_.clear(); } -void ImuProcess::Process2(LidarMeasureGroup &lidar_meas, StatesGroup &stat, PointCloudXYZI::Ptr cur_pcl_un_) -{ +void ImuProcess::Process(LidarMeasureGroup &lidar_meas, StatesGroup &state, + PointCloudXYZIN::Ptr cur_pcl_un_) { double t1, t2, t3; t1 = omp_get_wtime(); ROS_ASSERT(lidar_meas.lidar != nullptr); - if (!imu_en) - { - Forward_without_imu(lidar_meas, stat, *cur_pcl_un_); - return; - } MeasureGroup meas = lidar_meas.measures.back(); - if (imu_need_init) - { - double pcl_end_time = lidar_meas.lio_vio_flg == LIO ? meas.lio_time : meas.vio_time; - // lidar_meas.last_lio_update_time = pcl_end_time; + if (imu_need_init_) { + double pcl_end_time = meas.lio_time; - if (meas.imu.empty()) { return; }; + if (meas.imu.empty()) { + return; + }; /// The very first lidar frame - IMU_init(meas, stat, init_iter_num); - - imu_need_init = true; + ImuInit(meas, state, init_iter_num_); - last_imu = meas.imu.back(); + last_imu_ = meas.imu.back(); - if (init_iter_num > MAX_INI_COUNT) - { + if (init_iter_num_ > max_ini_count_) { // cov_acc *= pow(G_m_s2 / mean_acc.norm(), 2); - imu_need_init = false; - ROS_INFO("IMU Initials: Gravity: %.4f %.4f %.4f %.4f; acc covarience: " - "%.8f %.8f %.8f; gry covarience: %.8f %.8f %.8f \n", - stat.gravity[0], stat.gravity[1], stat.gravity[2], mean_acc.norm(), cov_acc[0], cov_acc[1], cov_acc[2], cov_gyr[0], cov_gyr[1], - cov_gyr[2]); - ROS_INFO("IMU Initials: ba covarience: %.8f %.8f %.8f; bg covarience: " - "%.8f %.8f %.8f", - cov_bias_acc[0], cov_bias_acc[1], cov_bias_acc[2], cov_bias_gyr[0], cov_bias_gyr[1], cov_bias_gyr[2]); - fout_imu.open(DEBUG_FILE_DIR("imu.txt"), ios::out); + imu_need_init_ = false; + ROS_INFO( + "IMU Initials: Gravity: %.4f %.4f %.4f %.4f; acc covarience: " + "%.8f %.8f %.8f; gry covarience: %.8f %.8f %.8f \n", + state.gravity[0], state.gravity[1], state.gravity[2], + mean_acc_.norm(), cov_acc_[0], cov_acc_[1], cov_acc_[2], cov_gyr_[0], + cov_gyr_[1], cov_gyr_[2]); + ROS_INFO( + "IMU Initials: ba covarience: %.8f %.8f %.8f; bg covarience: " + "%.8f %.8f %.8f", + cov_bias_acc_[0], cov_bias_acc_[1], cov_bias_acc_[2], + cov_bias_gyr_[0], cov_bias_gyr_[1], cov_bias_gyr_[2]); + fout_imu_.open(DEBUG_FILE_DIR("imu.txt"), std::ios::out); } return; } - UndistortPcl(lidar_meas, stat, *cur_pcl_un_); - // cout << "[ IMU ] undistorted point num: " << cur_pcl_un_->size() << endl; + UndistortPcl(lidar_meas, state, *cur_pcl_un_); } \ No newline at end of file diff --git a/src/LIVMapper.cpp b/src/LIVMapper.cpp index 15af3540..94ba8d49 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 @@ -12,309 +12,322 @@ which is included as part of this source code package. #include "LIVMapper.h" -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); +#include +#include - p_pre.reset(new Preprocess()); - p_imu.reset(new ImuProcess()); +LIVMapper::LIVMapper(ros::NodeHandle &nh, const std::string &camera_config) + : ext_t_(0, 0, 0), ext_r_(M3D::Identity()), camera_config_(camera_config) { + extrin_t_.assign(3, 0.0); + extrin_r_.assign(9, 0.0); + cameraextrin_t_.assign(3, 0.0); + cameraextrin_r_.assign(9, 0.0); - readParameters(nh); + 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"; + visual_sub_map_.reset(new PointCloudXYZIN()); + feats_undistort_.reset(new PointCloudXYZIN()); + feats_down_body_.reset(new PointCloudXYZIN()); + feats_down_world_.reset(new PointCloudXYZIN()); + pcl_w_wait_pub_.reset(new PointCloudXYZIN()); + pcl_wait_pub_.reset(new PointCloudXYZIN()); + pcl_wait_save_.reset(new PointCloudXYZRGB()); + pcl_wait_save_intensity_.reset(new PointCloudXYZIN()); + voxel_map_manager_.reset(new VoxelMapManager(voxel_config)); + vio_manager_.reset(new VIOManager()); + root_dir_ = ROOT_DIR; + InitializeFiles(); + InitializeComponents(); + path_.header.stamp = ros::Time::now(); + path_.header.frame_id = "camera_init"; + int lru_size = 1000000; + nh.param("lru_config/lru_size", lru_size, 1000000); + voxel_map_manager_->lru_size_ = lru_size; + vio_manager_->lru_size_ = lru_size; + LOG(WARNING) << "LRU size: " << lru_size; } 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/type", pcd_save_type, 0); - nh.param("image_save/img_save_en", img_save_en, false); - nh.param("image_save/interval", img_save_interval, 1); - - 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); +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/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/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/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/type", pcd_save_type_, 0); + nh.param("image_save/img_save_en", img_save_en_, false); + nh.param("image_save/interval", img_save_interval_, 1); + 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", extrin_t_, + std::vector()); + nh.param>("extrin_calib/extrinsic_R", extrin_r_, + std::vector()); + nh.param>("extrin_calib/Pcl", cameraextrin_t_, + std::vector()); + nh.param>("extrin_calib/Rcl", cameraextrin_r_, + std::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; + p_pre_->blind_sqr_ = p_pre_->blind_ * p_pre_->blind_; } -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; +void LIVMapper::InitializeComponents() { + downSize_filter_surf_.setLeafSize( + filter_size_surf_min_, filter_size_surf_min_, filter_size_surf_min_); + ext_t_ << VEC_FROM_ARRAY(extrin_t_); + ext_r_ << MAT_FROM_ARRAY(extrin_r_); + + voxel_map_manager_->extT_ << VEC_FROM_ARRAY(extrin_t_); + voxel_map_manager_->extR_ << MAT_FROM_ARRAY(extrin_r_); + // 载入相机参数 + YAML::Node camera_config = YAML::LoadFile(camera_config_); + if (!vk::camera_loader::loadFromYaml(camera_config, 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(ext_t_, ext_r_); + vio_manager_->SetLidarToCameraExtrinsic(cameraextrin_r_, cameraextrin_t_); + // vio_manager的state_和LIVMapper的state_和state_propagat_是一致的，但是voxel_map的需要手动同步 + 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_->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_->SetExtrinsic(ext_t_, ext_r_); + p_imu_->SetGyrCovScale(V3D(gyr_cov_, gyr_cov_, gyr_cov_)); + p_imu_->SetAccCovScale(V3D(acc_cov_, acc_cov_, acc_cov_)); + p_imu_->SetInvExpoCov(inv_expo_cov_); + p_imu_->SetGyrBiasCov(V3D(0.0001, 0.0001, 0.0001)); + p_imu_->SetAccBiasCov(V3D(0.0001, 0.0001, 0.0001)); + p_imu_->SetImuInitFrameNum(imu_int_frame_); + + if (!gravity_est_en_) p_imu_->DisableGravityEst(); + if (!ba_bg_est_en_) p_imu_->DisableBiasEst(); + if (!exposure_estimate_en_) p_imu_->DisableExposureEst(); + + slam_mode_ = img_en_ ? LIVO : ONLY_LIO; } -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; - } +void LIVMapper::InitializeFiles() { + if (pcd_save_en_ && colmap_output_en_) { + const std::string folder_path = + std::string(ROOT_DIR) + "/scripts/colmap_output.sh"; - int executionRet = system(folderPath.c_str()); - if (executionRet != 0) { - std::cerr << "Failed to execute the script." << std::endl; - return; - } + std::string chmod_command = "chmod +x " + folder_path; + + int chmod_ret = system(chmod_command.c_str()); + if (chmod_ret != 0) { + std::cerr << "Failed to set execute permissions for the script." + << std::endl; + return; + } + + int execution_ret = system(folder_path.c_str()); + if (execution_ret != 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_en) fout_lidar_pos.open(std::string(ROOT_DIR) + "Log/pcd/lidar_poses.txt", std::ios::out); - if(img_save_en) fout_visual_pos.open(std::string(ROOT_DIR) + "Log/image/image_poses.txt", 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 (colmap_output_en_) + fout_points_.open( + std::string(ROOT_DIR) + "Log/Colmap/sparse/0/points3D.txt", + std::ios::out); + if (pcd_save_en_) + fout_lidar_pos_.open(std::string(ROOT_DIR) + "Log/pcd/lidar_poses.txt", + std::ios::out); + if (img_save_en_) + fout_visual_pos_.open(std::string(ROOT_DIR) + "Log/image/image_poses.txt", + 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) -{ - 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); +void LIVMapper::InitializeSubscribersAndPublishers( + ros::NodeHandle &nh, image_transport::ImageTransport &it) { + sub_pcl_ = + p_pre_->lidar_type_ == AVIA + ? nh.subscribe(lid_topic_, 200000, &LIVMapper::LivoxCbk, this) + : nh.subscribe(lid_topic_, 200000, &LIVMapper::PointCloud2Cbk, this); + sub_imu_ = nh.subscribe(imu_topic_, 200000, &LIVMapper::ImuCbk, this); + sub_img_ = nh.subscribe(img_topic_, 200000, &LIVMapper::ImageCbk, this); + + pubLaser_cloud_full_res_ = + nh.advertise("/cloud_registered", 100); + pub_normal_ = nh.advertise( + "visualization_marker", 100); + pub_sub_visual_map_ = nh.advertise( + "/cloud_visual_sub_map_before", 100); + pub_laser_cloud_effect_ = + nh.advertise("/cloud_effected", 100); + pub_laser_cloud_map_ = + nh.advertise("/Laser_map", 100); + pub_odom_aft_mapped_ = + nh.advertise("/aft_mapped_to_init", 10); + pub_path_ = nh.advertise("/path", 10); + plane_pub_ = nh.advertise("/planner_normal", 1); + voxel_pub_ = nh.advertise("/voxels", 1); + pub_laser_cloud_dyn_ = + nh.advertise("/dyn_obj", 100); + pub_laser_cloud_dyn_rmed_ = + nh.advertise("/dyn_obj_removed", 100); + pub_laser_cloud_dyn_dbg_ = + nh.advertise("/dyn_obj_dbg_hist", 100); + mavros_pose_publisher_ = + nh.advertise("/mavros/vision_pose/pose", 10); + pub_image_ = it.advertise("/rgb_img", 1); + pub_imu_prop_odom_ = + nh.advertise("/LIVO2/imu_propagate", 10000); + imu_prop_timer_ = + nh.createTimer(ros::Duration(0.004), &LIVMapper::ImuPropCallback, this); + voxel_map_manager_->voxel_map_pub_ = + nh.advertise("/planes", 10000); } -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; +void LIVMapper::HandleFirstFrame() { + if (!first_frame_finished_) { + first_lidar_time_ = lidar_measures_.last_lio_update_time; + p_imu_->first_lidar_time_ = first_lidar_time_; // Only for IMU data log + first_frame_finished_ = true; + std::cout << "FIRST LIDAR FRAME!" << std::endl; } } -void LIVMapper::gravityAlignment() -{ - if (!p_imu->imu_need_init && !gravity_align_finished) - { +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; + V3D ez(0, 0, -1), gz(state_.gravity); + Eigen::Quaterniond g_q_i0 = Eigen::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() -{ - // double t0 = omp_get_wtime(); - - p_imu->Process2(LidarMeasures, _state, feats_undistort); - - if (gravity_align_en) gravityAlignment(); +void LIVMapper::ProcessImu() { + p_imu_->Process(lidar_measures_, state_, feats_undistort_); - state_propagat = _state; - voxelmap_manager->state_ = _state; - voxelmap_manager->feats_undistort_ = feats_undistort; + if (gravity_align_en_) GravityAlignment(); - // 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; + state_propagat_ = state_; + voxel_map_manager_->state_ = state_; + voxel_map_manager_->undistort_size_ = feats_undistort_->size(); } -void LIVMapper::stateEstimationAndMapping() -{ - switch (LidarMeasures.lio_vio_flg) - { - case VIO: - handleVIO(); - break; - case LIO: - case LO: - handleLIO(); - break; - } +void LIVMapper::StateEstimationAndMapping() { + HandleLIO(); + state_propagat_ = state_; + if (img_en_) 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)) - { +void LIVMapper::HandleVIO() { + euler_cur_ = RotMtoEuler(state_.rot_end); + fout_pre_ << std::setw(20) + << lidar_measures_.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; + + std::cout << "[ VIO ] Raw feature num: " << pcl_w_wait_pub_->points.size() + << std::endl; + + if (fabs((lidar_measures_.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); + vio_manager_->ProcessFrame(lidar_measures_.measures.back().img, pv_list_, + voxel_map_manager_->vm_map_); - if (imu_prop_enable) - { - ekf_finish_once = true; - latest_ekf_state = _state; - latest_ekf_time = LidarMeasures.last_lio_update_time; - state_update_flg = true; + // vio_manager_->ProcessFrame( + // Lidar_measures_.measures.back().img, pv_list_, + // voxelmap_manager_->voxel_map_, Lidar_measures_.last_lio_update_time - + // first_lidar_time_); + + if (imu_prop_enable_) { + ekf_finish_once_ = true; + latest_ekf_state_ = state_; + latest_ekf_time_ = lidar_measures_.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++) + // for (int i = 0; i < size_sub_map; i++) // { // PointType temp_map; // temp_map.x = vio_manager->visual_sub_map_cur[i]->pos_[0]; @@ -324,326 +337,383 @@ void LIVMapper::handleVIO() // 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; + PublishFrameWorld(pubLaser_cloud_full_res_, vio_manager_); + PublishImgRGB(pub_image_, vio_manager_); + + euler_cur_ = RotMtoEuler(state_.rot_end); + fout_out_ << std::setw(20) + << lidar_measures_.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::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)) - { +void LIVMapper::HandleLIO() { + euler_cur_ = RotMtoEuler(state_.rot_end); + fout_pre_ << std::setw(20) + << lidar_measures_.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 (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); - + downSize_filter_surf_.setInputCloud(feats_undistort_); + downSize_filter_surf_.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(); + feats_down_size_ = feats_down_body_->points.size(); + voxel_map_manager_->feats_down_body_ = feats_down_body_; + voxel_map_manager_->feats_down_size_ = feats_down_size_; + + if (!lidar_map_inited_) { + // 第一帧，建立VoxelMap + lidar_map_inited_ = true; + // voxelmap_manager_->BuildVoxelMap(); + TransformLidar(state_.rot_end, state_.pos_end, feats_down_body_, + feats_down_world_); + voxel_map_manager_->BuildVoxelMapLRU(feats_down_world_); } double t1 = omp_get_wtime(); - - voxelmap_manager->StateEstimation(state_propagat); - _state = voxelmap_manager->state_; - _pv_list = voxelmap_manager->pv_list_; + // 位姿估计 + voxel_map_manager_->StateEstimation(state_propagat_); + state_ = voxel_map_manager_->state_; 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 (imu_prop_enable_) { + ekf_finish_once_ = true; + latest_ekf_state_ = state_; + latest_ekf_time_ = lidar_measures_.last_lio_update_time; + state_update_flg_ = true; } - if (pose_output_en) - { + 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); + std::ofstream out_file, evo_file; + if (!pos_opend) { + evo_file.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 - { - evoFile.open(std::string(ROOT_DIR) + "Log/result/" + seq_name + ".txt", std::ios::app); - if (!evoFile.is_open()) ROS_ERROR("open fail\n"); + if (!evo_file.is_open()) ROS_ERROR("open fail\n"); + } else { + evo_file.open(std::string(ROOT_DIR) + "Log/result/" + seq_name_ + ".txt", + std::ios::app); + if (!evo_file.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; + Eigen::Quaterniond q(state_.rot_end); + evo_file << std::fixed; + evo_file << lidar_measures_.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); + + euler_cur_ = RotMtoEuler(state_.rot_end); + geo_quat_ = tf::createQuaternionMsgFromRollPitchYaw( + euler_cur_(0), euler_cur_(1), euler_cur_(2)); + PublishOdometry(pub_odom_aft_mapped_); 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 + PointCloudXYZIN::Ptr world_lidar(new PointCloudXYZIN()); + TransformLidar(state_.rot_end, state_.pos_end, feats_down_body_, world_lidar); + for (size_t i = 0; i < world_lidar->points.size(); i++) { + voxel_map_manager_->pv_list_[i].point_w << world_lidar->points[i].x, + world_lidar->points[i].y, world_lidar->points[i].z; + M3D point_crossmat = voxel_map_manager_->cross_mat_list_[i]; + M3D var = voxel_map_manager_->body_cov_list_[i]; + var = (state_.rot_end * ext_r_) * var * + (state_.rot_end * ext_r_).transpose() + + (-point_crossmat) * state_.cov.block<3, 3>(0, 0) * + (-point_crossmat).transpose() + + state_.cov.block<3, 3>(3, 3); + voxel_map_manager_->pv_list_[i].var = var; } - voxelmap_manager->UpdateVoxelMap(voxelmap_manager->pv_list_); + // voxelmap_manager_->UpdateVoxelMap(voxelmap_manager_->pv_list_); + voxel_map_manager_->UpdateVoxelMapLRU(voxel_map_manager_->pv_list_); std::cout << "[ LIO ] Update Voxel Map" << std::endl; - _pv_list = voxelmap_manager->pv_list_; - + pv_list_ = voxel_map_manager_->pv_list_; + double t4 = omp_get_wtime(); - if(voxelmap_manager->config_setting_.map_sliding_en) - { - voxelmap_manager->mapSliding(); + if (voxel_map_manager_->config_setting_.map_sliding_en) { + voxel_map_manager_->MapSliding(); } - - PointCloudXYZI::Ptr laserCloudFullRes(dense_map_en ? feats_undistort : feats_down_body); + + PointCloudXYZIN::Ptr laserCloudFullRes(dense_map_en ? feats_undistort_ + : feats_down_body_); int size = laserCloudFullRes->points.size(); - PointCloudXYZI::Ptr laserCloudWorld(new PointCloudXYZI(size, 1)); + PointCloudXYZIN::Ptr cloud_world(new PointCloudXYZIN(size, 1)); - for (int i = 0; i < size; i++) - { - RGBpointBodyToWorld(&laserCloudFullRes->points[i], &laserCloudWorld->points[i]); + for (int i = 0; i < size; i++) { + RGBpointBodyToWorld(&laserCloudFullRes->points[i], &cloud_world->points[i]); + } + *pcl_w_wait_pub_ = *cloud_world; + + if (!img_en_) PublishFrameWorld(pubLaser_cloud_full_res_, vio_manager_); + if (pub_effect_point_en_) + PublishEffectWorld(pub_laser_cloud_effect_, voxel_map_manager_->ptpl_list_); + if (voxel_map_manager_->config_setting_.is_pub_plane_map_) { + // voxelmap_manager_->PubVoxelMap(); + voxel_map_manager_->PubVoxelMapLRU(); } - *pcl_w_wait_pub = *laserCloudWorld; - - 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"); + PublishPath(pub_path_); + PublishMavros(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;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; + 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) + << lidar_measures_.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() -{ - 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"; +void LIVMapper::SavePCD() { + 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; - if (img_en) - { - pcl::PointCloud::Ptr downsampled_cloud(new pcl::PointCloud); + 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.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; + + 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; + } 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; } } } -void LIVMapper::run() -{ +// 主函数 +void LIVMapper::Run() { ros::Rate rate(5000); - while (ros::ok()) - { + while (ros::ok()) { ros::spinOnce(); - if (!sync_packages(LidarMeasures)) - { + if (!SyncPackages(lidar_measures_)) { rate.sleep(); continue; } - handleFirstFrame(); - - processImu(); + HandleFirstFrame(); - // if (!p_imu->imu_time_init) continue; + ProcessImu(); - stateEstimationAndMapping(); + StateEstimationAndMapping(); } - savePCD(); + 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; +void LIVMapper::PropImuOnce(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; - M3D Exp_f = Exp(angvel_avr, dt); + M3D exp_f = Exp(angvel_avr, dt); /* propogation of IMU attitude */ - imu_prop_state.rot_end = imu_prop_state.rot_end * Exp_f; + 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]); + 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; + 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; } -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()) - { +void LIVMapper::ImuPropCallback(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) - { - imu_propagate = latest_ekf_state; + 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(); + 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; + 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); + // 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); + PropImuOnce(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; + 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); + PropImuOnce(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); + 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(); + pub_imu_prop_odom_.publish(imu_prop_odom_); } - mtx_buffer_imu_prop.unlock(); + 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); +void LIVMapper::TransformLidar(const Eigen::Matrix3d rot, + const Eigen::Vector3d t, + const PointCloudXYZIN::Ptr &input_cloud, + PointCloudXYZIN::Ptr &trans_cloud) { + PointCloudXYZIN().swap(*trans_cloud); trans_cloud->reserve(input_cloud->size()); - for (size_t i = 0; i < input_cloud->size(); i++) - { + 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; + p = (rot * (ext_r_ * p + ext_t_) + t); + PointXYZIN pi; pi.x = p(0); pi.y = p(1); pi.z = p(2); @@ -652,47 +722,48 @@ void LIVMapper::transformLidar(const Eigen::Matrix3d rot, const Eigen::Vector3d } } -void LIVMapper::pointBodyToWorld(const PointType &pi, PointType &po) -{ +void LIVMapper::PointBodyToWorld(const PointXYZIN &pi, PointXYZIN &po) { V3D p_body(pi.x, pi.y, pi.z); - V3D p_global(_state.rot_end * (extR * p_body + extT) + _state.pos_end); + V3D p_global(state_.rot_end * (ext_r_ * p_body + ext_t_) + 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 Eigen::Matrix &pi, + Eigen::Matrix &po) { V3D p_body(pi[0], pi[1], pi[2]); - V3D p_global(_state.rot_end * (extR * p_body + extT) + _state.pos_end); + V3D p_global(state_.rot_end * (ext_r_ * p_body + ext_t_) + 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 +Eigen::Matrix LIVMapper::PointBodyToWorld( + const Eigen::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]); + p = (state_.rot_end * (ext_r_ * p + ext_t_) + state_.pos_end); + Eigen::Matrix po(p[0], p[1], p[2]); return po; } -void LIVMapper::RGBpointBodyToWorld(PointType const *const pi, PointType *const po) -{ +void LIVMapper::RGBpointBodyToWorld(PointXYZIN const *const pi, + PointXYZIN *const po) { V3D p_body(pi->x, pi->y, pi->z); - V3D p_global(_state.rot_end * (extR * p_body + extT) + _state.pos_end); + V3D p_global(state_.rot_end * (ext_r_ * p_body + ext_t_) + state_.pos_end); po->x = p_global(0); po->y = p_global(1); po->z = p_global(2); po->intensity = pi->intensity; } -void LIVMapper::RGBpointBodyLidarToIMU(PointType const *const pi, PointType *const po) -{ +void LIVMapper::RGBpointBodyLidarToIMU(PointXYZIN const *const pi, + PointXYZIN *const po) { V3D p_body_lidar(pi->x, pi->y, pi->z); - V3D p_body_imu(extR * p_body_lidar + extT); + V3D p_body_imu(ext_r_ * p_body_lidar + ext_t_); po->x = p_body_imu(0); po->y = p_body_imu(1); @@ -700,672 +771,582 @@ void LIVMapper::RGBpointBodyLidarToIMU(PointType const *const pi, PointType *con po->intensity = pi->intensity; } -void LIVMapper::standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg) -{ - if (!lidar_en) return; - mtx_buffer.lock(); +void LIVMapper::PointCloud2Cbk(const sensor_msgs::PointCloud2::ConstPtr &msg) { + mtx_buffer_.lock(); - double cur_head_time = msg->header.stamp.toSec() + lidar_time_offset; + 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(); + PointCloudXYZIN::Ptr ptr(new PointCloudXYZIN()); + 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) +void LIVMapper::LivoxCbk(const livox_ros_driver::CustomMsg::ConstPtr &msg_in) { + 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); + // 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; + 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) - { + if (cur_head_time < last_timestamp_lidar_) { ROS_ERROR("lidar loop back, clear buffer"); - lid_raw_data_buffer.clear(); + 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); + PointCloudXYZIN::Ptr ptr(new PointCloudXYZIN()); + p_pre_->Process(msg, ptr); if (!ptr || ptr->empty()) { ROS_ERROR("Received an empty point cloud"); - mtx_buffer.unlock(); + mtx_buffer_.unlock(); return; } - lid_raw_data_buffer.push_back(ptr); - lid_header_time_buffer.push_back(cur_head_time); - last_timestamp_lidar = cur_head_time; + 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(); + mtx_buffer_.unlock(); + sig_buffer_.notify_all(); } -void LIVMapper::imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in) -{ - if (!imu_en) return; - - if (last_timestamp_lidar < 0.0) return; +void LIVMapper::ImuCbk(const sensor_msgs::Imu::ConstPtr &msg_in) { + 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); + 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 (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 (ros_driver_fix_en) timestamp += std::round(last_timestamp_lidar - timestamp); + if (ros_driver_fix_en_) + timestamp += std::round(last_timestamp_lidar_ - timestamp); msg->header.stamp = ros::Time().fromSec(timestamp); - mtx_buffer.lock(); + 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); + 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 (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(); + // 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; + last_timestamp_imu_ = timestamp; - imu_buffer.push_back(msg); + 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(); + mtx_buffer_.unlock(); + if (imu_prop_enable_) { + mtx_buffer_imu_prop_.lock(); + if (imu_prop_enable_ && !p_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(); + sig_buffer_.notify_all(); } -cv::Mat LIVMapper::getImageFromMsg(const sensor_msgs::ImageConstPtr &img_msg) -{ +cv::Mat LIVMapper::GetImageFromMsg(const sensor_msgs::ImageConstPtr &img_msg) { cv::Mat img; img = cv_bridge::toCvCopy(img_msg, "bgr8")->image; return img; } -void LIVMapper::img_cbk(const sensor_msgs::ImageConstPtr &msg_in) -{ - if (!img_en) return; +// static int i = 0; +void LIVMapper::ImageCbk(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) + // 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); + // 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 (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; + 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 (last_timestamp_lidar_ < 0) return; - if (msg_header_time < last_timestamp_img) - { + if (msg_header_time < last_timestamp_img_) { ROS_ERROR("image loop back. \n"); return; } - mtx_buffer.lock(); + mtx_buffer_.lock(); - double img_time_correct = msg_header_time; // last_timestamp_lidar + 0.105; + double img_time_correct = msg_header_time; // last_timestamp_lidar + 0.105; - if (img_time_correct - last_timestamp_img < 0.02) - { + 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(); + 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); + 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; + last_timestamp_img_ = img_time_correct; // cv::imshow("img", img); // cv::waitKey(1); // cout<<"last_timestamp_img:::"<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 - } +bool LIVMapper::SyncPackages(LidarMeasureGroup &meas) { + if (lid_raw_data_buffer_.empty()) return false; + if (img_buffer_.empty() && img_en_) return false; + if (imu_buffer_.empty()) return false; + + switch (slam_mode_) { + case ONLY_LIO: { + 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 (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; - } + if (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. + 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()) { + 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(); + mtx_buffer_.unlock(); + sig_buffer_.notify_all(); + + meas.measures.push_back(m); + // ROS_INFO("ONlY HAS LiDAR and IMU, NO IMAGE!"); + lidar_pushed_ = false; // sync one whole lidar scan. + return true; - m.imu.clear(); - m.lio_time = meas.lidar_frame_end_time; - mtx_buffer.lock(); - while (!imu_buffer.empty()) - { - if (imu_buffer.front()->header.stamp.toSec() > meas.lidar_frame_end_time) break; - m.imu.push_back(imu_buffer.front()); - imu_buffer.pop_front(); + break; } - lid_raw_data_buffer.pop_front(); - lid_header_time_buffer.pop_front(); - 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 LIVO: - { - /*** 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(); - img_time_buffer.pop_front(); + case LIVO: { + // LIVO模式下，LIO和VIO的更新时间相同，LIO在前，VIO紧随其后。 + // 注意：此处整理的是LIO使用的数据 + // 图像开始时间+曝光时间 + double img_capture_time = img_time_buffer_.front() + exposure_time_init_; + // 存在图像话题，但图像话题时间戳大于激光雷达结束时间此时处理激光雷达话题。LIO更新后，meas.lidar_frame_end_time将被刷新。 + if (meas.last_lio_update_time < 0.0) { + meas.last_lio_update_time = lid_header_time_buffer_.front(); + } + + // 取雷达和IMU最新的时间 + 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(); + 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); + // 图像的获取时间大于雷达和IMU最新的时间，等待雷达和IMU数据 + if (img_capture_time > lid_newest_time || + img_capture_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(); + // 处理IMU数据 m.lio_time = img_capture_time; - mtx_buffer.lock(); - while (!imu_buffer.empty()) - { - if (imu_buffer.front()->header.stamp.toSec() > m.lio_time) break; + 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()); + 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()); + imu_buffer_.pop_front(); } - mtx_buffer.unlock(); - sig_buffer.notify_all(); + mtx_buffer_.unlock(); + sig_buffer_.notify_all(); + // 处理激光雷达数据 + // 上一帧的next移动当前帧的cur，同时清理next *(meas.pcl_proc_cur) = *(meas.pcl_proc_next); - PointCloudXYZI().swap(*meas.pcl_proc_next); + PointCloudXYZIN().swap(*meas.pcl_proc_next); - int lid_frame_num = lid_raw_data_buffer.size(); + 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()); + 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++) - { + // 时间小于图像获取时间的点，放入当前帧 + 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; + 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 - { + } 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(); + 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(); - 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(); + if (img_en_) { + // 注意：此处整理的是VIO使用的数据 + // 只添加图片 + m.img = img_buffer_.front(); + mtx_buffer_.lock(); + 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 !!"); + default: { + printf("!! WRONG SLAM TYPE !!"); return false; } - // 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"); } -void LIVMapper::publish_img_rgb(const image_transport::Publisher &pubImage, VIOManagerPtr vio_manager) -{ - cv::Mat img_rgb = vio_manager->img_cp; +void LIVMapper::PublishImgRGB(const image_transport::Publisher &pub_image, + 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()); + pub_image.publish(out_msg.toImageMsg()); } // Provide output format for LiDAR-visual BA -void LIVMapper::publish_frame_world(const ros::Publisher &pubLaserCloudFullRes, VIOManagerPtr vio_manager) -{ - if (pcl_w_wait_pub->empty()) return; - PointCloudXYZRGB::Ptr laserCloudWorldRGB(new PointCloudXYZRGB()); - static int pub_num = 1; - pub_num++; - - if (LidarMeasures.lio_vio_flg == VIO) - { - *pcl_wait_pub += *pcl_w_wait_pub; - if(pub_num >= pub_scan_num) - { +void LIVMapper::PublishFrameWorld( + const ros::Publisher &pub_laser_cloud_full_res, VIOManagerPtr vio_manager) { + if (pcl_w_wait_pub_->empty()) return; + PointCloudXYZRGB::Ptr cloud_world_rgb(new PointCloudXYZRGB()); + if (img_en_) { + static int pub_num = 1; + pub_num++; + *pcl_wait_pub_ += *pcl_w_wait_pub_; + if (pub_num >= pub_scan_num_) { pub_num = 1; - size_t size = pcl_wait_pub->points.size(); - laserCloudWorldRGB->reserve(size); + size_t size = pcl_wait_pub_->points.size(); + cloud_world_rgb->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; + cv::Mat img_rgb = vio_manager->img_rgb_; + for (size_t i = 0; i < size; i++) { + PointTypeRGB point_rgb; + point_rgb.x = pcl_wait_pub_->points[i].x; + point_rgb.y = pcl_wait_pub_->points[i].y; + point_rgb.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 + 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); + V3F pixel = vio_manager->GetInterpolatedPixel(img_rgb, pc); + point_rgb.r = pixel[2]; + point_rgb.g = pixel[1]; + point_rgb.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_) + cloud_world_rgb->push_back(point_rgb); } } + } else { + pub_num++; } } /*** Publish Frame ***/ - sensor_msgs::PointCloud2 laserCloudmsg; - if (slam_mode_ == LIVO && LidarMeasures.lio_vio_flg == VIO) - { - pcl::toROSMsg(*laserCloudWorldRGB, laserCloudmsg); + sensor_msgs::PointCloud2 cloud_msg; + if (img_en_) { + // cout << "RGB pointcloud size: " << cloud_world_rgb->size() << endl; + pcl::toROSMsg(*cloud_world_rgb, cloud_msg); + } else { + pcl::toROSMsg(*pcl_w_wait_pub_, cloud_msg); } - if (slam_mode_ == ONLY_LIO || slam_mode_ == ONLY_LO) - { - 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); + cloud_msg.header.stamp = ros::Time::now(); //.fromSec(last_timestamp_lidar); + cloud_msg.header.frame_id = "camera_init"; + pub_laser_cloud_full_res.publish(cloud_msg); /**************** save map ****************/ /* 1. make sure you have enough memories /* 2. noted that pcd save will influence the real-time performences **/ - double update_time = 0.0; - if (LidarMeasures.lio_vio_flg == VIO) { - update_time = LidarMeasures.measures.back().vio_time; - } else { // LIO / LO - update_time = LidarMeasures.measures.back().lio_time; - } + double update_time = lidar_measures_.measures.back().lio_time; std::stringstream ss_time; ss_time << std::fixed << std::setprecision(6) << update_time; - if (pcd_save_en) - { + if (pcd_save_en_) { static int scan_wait_num = 0; - switch (pcd_save_type) - { + switch (pcd_save_type_) { case 0: /** world frame **/ - if (slam_mode_ == LIVO) - { - *pcl_wait_save += *laserCloudWorldRGB; + if (img_en_) { + *pcl_wait_save_ += *cloud_world_rgb; + } else { + *pcl_wait_save_intensity_ += *pcl_w_wait_pub_; } - else - { - *pcl_wait_save_intensity += *pcl_w_wait_pub; - } - if(LidarMeasures.lio_vio_flg == LIO || LidarMeasures.lio_vio_flg == LO) scan_wait_num++; + scan_wait_num++; break; case 1: /** body frame **/ - if (LidarMeasures.lio_vio_flg == LIO || LidarMeasures.lio_vio_flg == LO) - { - int size = feats_undistort->points.size(); - PointCloudXYZI::Ptr laserCloudBody(new PointCloudXYZI(size, 1)); - for (int i = 0; i < size; i++) - { - RGBpointBodyLidarToIMU(&feats_undistort->points[i], &laserCloudBody->points[i]); - } - *pcl_wait_save_intensity += *laserCloudBody; - scan_wait_num++; - cout << "save body frame points: " << pcl_wait_save_intensity->points.size() << endl; + { + int size = feats_undistort_->points.size(); + PointCloudXYZIN::Ptr cloud_body(new PointCloudXYZIN(size, 1)); + for (int i = 0; i < size; i++) { + RGBpointBodyLidarToIMU(&feats_undistort_->points[i], + &cloud_body->points[i]); } - pcd_save_interval = 1; - + *pcl_wait_save_intensity_ += *cloud_body; + std::cout << "save body frame points: " + << pcl_wait_save_intensity_->points.size() << std::endl; + } + pcd_save_interval_ = 1; + scan_wait_num++; break; default: - pcd_save_interval = 1; + pcd_save_interval_ = 1; scan_wait_num++; break; } - if ((pcl_wait_save->size() > 0 || pcl_wait_save_intensity->size() > 0) && pcd_save_interval > 0 && scan_wait_num >= pcd_save_interval) - { - string all_points_dir(string(string(ROOT_DIR) + "Log/pcd/") + ss_time.str() + string(".pcd")); + if ((pcl_wait_save_->size() > 0 || pcl_wait_save_intensity_->size() > 0) && + pcd_save_interval_ > 0 && scan_wait_num >= pcd_save_interval_) { + std::string all_points_dir( + std::string(std::string(ROOT_DIR) + "Log/pcd/") + ss_time.str() + + std::string(".pcd")); pcl::PCDWriter pcd_writer; - cout << "current scan saved to " << all_points_dir << endl; - if (pcl_wait_save->points.size() > 0) - { - pcd_writer.writeBinary(all_points_dir, *pcl_wait_save); // pcl::io::savePCDFileASCII(all_points_dir, *pcl_wait_save); - PointCloudXYZRGB().swap(*pcl_wait_save); + // 保存点云 + std::cout << "current scan saved to " << all_points_dir << std::endl; + if (pcl_wait_save_->points.size() > 0) { + pcd_writer.writeBinary( + all_points_dir, + *pcl_wait_save_); // pcl::io::savePCDFileASCII(all_points_dir, + // *pcl_wait_save); + PointCloudXYZRGB().swap(*pcl_wait_save_); } - if(pcl_wait_save_intensity->points.size() > 0) - { - pcd_writer.writeBinary(all_points_dir, *pcl_wait_save_intensity); - PointCloudXYZI().swap(*pcl_wait_save_intensity); + if (pcl_wait_save_intensity_->points.size() > 0) { + pcd_writer.writeBinary(all_points_dir, *pcl_wait_save_intensity_); + PointCloudXYZIN().swap(*pcl_wait_save_intensity_); } scan_wait_num = 0; } - - if(LidarMeasures.lio_vio_flg == LIO || LidarMeasures.lio_vio_flg == LO) - { - Eigen::Quaterniond q(_state.rot_end); - fout_lidar_pos << std::fixed << std::setprecision(6); - fout_lidar_pos << LidarMeasures.measures.back().lio_time << " " << _state.pos_end[0] << " " << _state.pos_end[1] << " " << _state.pos_end[2] << " " << q.x() << " " << q.y() << " " << q.z() - << " " << q.w() << " " << endl; - } + + // 保存位姿 + Eigen::Quaterniond q(state_.rot_end); + fout_lidar_pos_ << std::fixed << std::setprecision(6); + fout_lidar_pos_ << lidar_measures_.measures.back().lio_time << " " + << state_.pos_end[0] << " " << state_.pos_end[1] << " " + << state_.pos_end[2] << " " << q.x() << " " << q.y() << " " + << q.z() << " " << q.w() << " " << std::endl; } - if (img_save_en && LidarMeasures.lio_vio_flg == VIO) - { + + // 保存图片 + if (img_save_en_) { static int img_wait_num = 0; img_wait_num++; - if (img_save_interval > 0 && img_wait_num >= img_save_interval) - { - imwrite(string(string(ROOT_DIR) + "Log/image/") + ss_time.str() + string(".png"), vio_manager->img_rgb); - - Eigen::Quaterniond q(_state.rot_end); - fout_visual_pos << std::fixed << std::setprecision(6); - fout_visual_pos << LidarMeasures.measures.back().vio_time << " " << _state.pos_end[0] << " " << _state.pos_end[1] << " " << _state.pos_end[2] << " " - << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << std::endl; + if (img_save_interval_ > 0 && img_wait_num >= img_save_interval_) { + imwrite(std::string(std::string(ROOT_DIR) + "Log/image/") + + ss_time.str() + std::string(".png"), + vio_manager->img_rgb_); + + Eigen::Quaterniond q(state_.rot_end); + fout_visual_pos_ << std::fixed << std::setprecision(6); + fout_visual_pos_ << lidar_measures_.measures.back().vio_time << " " + << state_.pos_end[0] << " " << state_.pos_end[1] << " " + << state_.pos_end[2] << " " << q.x() << " " << q.y() + << " " << q.z() << " " << q.w() << std::endl; img_wait_num = 0; } } - if(laserCloudWorldRGB->size() > 0) PointCloudXYZI().swap(*pcl_wait_pub); - if(LidarMeasures.lio_vio_flg == VIO) PointCloudXYZI().swap(*pcl_w_wait_pub); + if (cloud_world_rgb->size() > 0) PointCloudXYZIN().swap(*pcl_wait_pub_); + PointCloudXYZIN().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); +void LIVMapper::PublishVisualSubMap(const ros::Publisher &pub_sub_visual_map) { + PointCloudXYZIN::Ptr cloud_full_res(visual_sub_map_); + int size = cloud_full_res->points.size(); + if (size == 0) return; + PointCloudXYZIN::Ptr sub_pcl_visual_map_pub(new PointCloudXYZIN()); + *sub_pcl_visual_map_pub = *cloud_full_res; + if (1) { + sensor_msgs::PointCloud2 cloud_msg; + pcl::toROSMsg(*sub_pcl_visual_map_pub, cloud_msg); + cloud_msg.header.stamp = ros::Time::now(); + cloud_msg.header.frame_id = "camera_init"; + pub_sub_visual_map.publish(cloud_msg); } } -void LIVMapper::publish_effect_world(const ros::Publisher &pubLaserCloudEffect, const std::vector &ptpl_list) -{ +void LIVMapper::PublishEffectWorld(const ros::Publisher &pub_cloud_effect, + 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++) - { + PointCloudXYZIN::Ptr laserCloudWorld(new PointCloudXYZIN(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); + sensor_msgs::PointCloud2 cloud_full_res; + pcl::toROSMsg(*laserCloudWorld, cloud_full_res); + cloud_full_res.header.stamp = ros::Time::now(); + cloud_full_res.header.frame_id = "camera_init"; + pub_cloud_effect.publish(cloud_full_res); } -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; +template +void LIVMapper::SetPosestamp(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 = geo_quat_.x; + out.orientation.y = geo_quat_.y; + out.orientation.z = geo_quat_.z; + out.orientation.w = geo_quat_.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); +void LIVMapper::PublishOdometry(const ros::Publisher &pub_odom) { + odom_aft_mapped_.header.frame_id = "camera_init"; + odom_aft_mapped_.child_frame_id = "aft_mapped"; + odom_aft_mapped_.header.stamp = + ros::Time::now(); //.ros::Time()fromSec(last_timestamp_lidar); + SetPosestamp(odom_aft_mapped_.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.setOrigin( + tf::Vector3(state_.pos_end(0), state_.pos_end(1), state_.pos_end(2))); + q.setW(geo_quat_.w); + q.setX(geo_quat_.x); + q.setY(geo_quat_.y); + q.setZ(geo_quat_.z); transform.setRotation(q); - br.sendTransform( tf::StampedTransform(transform, odomAftMapped.header.stamp, "camera_init", "aft_mapped") ); - pubOdomAftMapped.publish(odomAftMapped); + br.sendTransform(tf::StampedTransform( + transform, odom_aft_mapped_.header.stamp, "camera_init", "aft_mapped")); + pub_odom.publish(odom_aft_mapped_); } -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); +void LIVMapper::PublishMavros(const ros::Publisher &mavros_pose_publisher) { + msg_body_pose_.header.stamp = ros::Time::now(); + msg_body_pose_.header.frame_id = "camera_init"; + SetPosestamp(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); -} \ No newline at end of file +void LIVMapper::PublishPath(const ros::Publisher pub_path) { + SetPosestamp(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_); + pub_path.publish(path_); +} diff --git a/src/frame.cpp b/src/frame.cpp deleted file mode 100644 index 2a0b1268..00000000 --- a/src/frame.cpp +++ /dev/null @@ -1,65 +0,0 @@ -/* -This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. - -Developer: Chunran Zheng - -For commercial use, please contact me at or -Prof. Fu Zhang at . - -This file is subject to the terms and conditions outlined in the 'LICENSE' file, -which is included as part of this source code package. -*/ - -#include -#include "feature.h" -#include "frame.h" -#include "visual_point.h" -#include -#include -#include -#include - -int Frame::frame_counter_ = 0; - -Frame::Frame(vk::AbstractCamera *cam, const cv::Mat &img) - : id_(frame_counter_++), - cam_(cam) -{ - initFrame(img); -} - -Frame::~Frame() -{ - std::for_each(fts_.begin(), fts_.end(), [&](Feature *i) { delete i; }); -} - -void Frame::initFrame(const cv::Mat &img) -{ - if (img.empty()) { throw std::runtime_error("Frame: provided image is empty"); } - - 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; -} - -/// Utility functions for the Frame class -namespace frame_utils -{ - -void createImgPyramid(const cv::Mat &img_level_0, int n_levels, ImgPyr &pyr) -{ - pyr.resize(n_levels); - pyr[0] = img_level_0; - for (int i = 1; i < n_levels; ++i) - { - pyr[i] = cv::Mat(pyr[i - 1].rows / 2, pyr[i - 1].cols / 2, CV_8U); - vk::halfSample(pyr[i - 1], pyr[i]); - } -} - -} // namespace frame_utils diff --git a/src/image_frame.cpp b/src/image_frame.cpp new file mode 100644 index 00000000..641e8f49 --- /dev/null +++ b/src/image_frame.cpp @@ -0,0 +1,148 @@ +/* +This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. + +Developer: Chunran Zheng + +For commercial use, please contact me at or +Prof. Fu Zhang at . + +This file is subject to the terms and conditions outlined in the 'LICENSE' file, +which is included as part of this source code package. +*/ + +#include "image_frame.h" + +#include +#include +#include + +#include +#include + +VisualPoint::VisualPoint(const Eigen::Vector3d &pos) + : pos_(pos), + previous_normal_(Eigen::Vector3d::Zero()), + normal_(Eigen::Vector3d::Zero()), + is_converged_(false), + is_normal_initialized_(false), + has_ref_patch_(false) {} + +VisualPoint::~VisualPoint() { + for (auto it = obs_.begin(), ite = obs_.end(); it != ite; ++it) { + delete (*it); + } + obs_.clear(); + ref_patch = nullptr; +} + +void VisualPoint::addFrameRef(Feature *ftr) { obs_.push_front(ftr); } + +void VisualPoint::deleteFeatureRef(Feature *ftr) { + if (ref_patch == ftr) { + ref_patch = nullptr; + has_ref_patch_ = false; + } + for (auto it = obs_.begin(), ite = obs_.end(); it != ite; ++it) { + if ((*it) == ftr) { + delete ((*it)); + obs_.erase(it); + return; + } + } +} + +bool VisualPoint::getCloseViewObs(const Eigen::Vector3d &framepos, + Feature *&ftr, + const Eigen::Vector2d &cur_px) const { + // TODO: get frame with same point of view AND same pyramid level! + if (obs_.size() <= 0) return false; + + Eigen::Vector3d obs_dir(framepos - pos_); + obs_dir.normalize(); + auto min_it = obs_.begin(); + double min_cos_angle = 0; + for (auto it = obs_.begin(), ite = obs_.end(); it != ite; ++it) { + Eigen::Vector3d dir((*it)->T_f_w_.inverse().translation() - pos_); + dir.normalize(); + double cos_angle = obs_dir.dot(dir); + if (cos_angle > min_cos_angle) { + min_cos_angle = cos_angle; + min_it = it; + } + } + ftr = *min_it; + + // Vector2d ftr_px = ftr->px_; + // double pixel_dist = (cur_px-ftr_px).norm(); + + // if(pixel_dist > 200) + // { + // ROS_ERROR("The pixel dist exceeds 200."); + // return false; + // } + + if (min_cos_angle < + 0.5) // assume that observations larger than 60° are useless 0.5 + { + // ROS_ERROR("The obseved angle is larger than 60°."); + return false; + } + + return true; +} + +void VisualPoint::findMinScoreFeature(const Eigen::Vector3d &framepos, + Feature *&ftr) const { + auto min_it = obs_.begin(); + float min_score = std::numeric_limits::max(); + + for (auto it = obs_.begin(), ite = obs_.end(); it != ite; ++it) { + if ((*it)->score_ < min_score) { + min_score = (*it)->score_; + min_it = it; + } + } + ftr = *min_it; +} + +void VisualPoint::deleteNonRefPatchFeatures() { + for (auto it = obs_.begin(); it != obs_.end();) { + if (*it != ref_patch) { + delete *it; + it = obs_.erase(it); + } else { + ++it; + } + } +} + +int ImageFrame::frame_counter_ = 0; + +ImageFrame::ImageFrame(vk::AbstractCamera *cam, const cv::Mat &img) + : id_(frame_counter_++), cam_(cam) { + initFrame(img); +} + +ImageFrame::~ImageFrame() { + std::for_each(fts_.begin(), fts_.end(), [&](Feature *i) { delete i; }); +} + +void ImageFrame::initFrame(const cv::Mat &img) { + if (img.empty()) { + throw std::runtime_error("Frame: provided image is empty"); + } + + if (img.cols != cam_->width() || img.rows != cam_->height()) { + std::cout << "Image size: " << img.cols << "x" << img.rows << std::endl; + std::cout << "Camera model size: " << cam_->width() << "x" << cam_->height() + << std::endl; + 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; +} \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 135b3363..d1329d43 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,12 +1,16 @@ #include "LIVMapper.h" +#include "glogger.h" -int main(int argc, char **argv) -{ +DEFINE_string(camera_config, "config/camera_fisheye_HILTI22.yaml", "camera config file."); + +int main(int argc, char **argv) { + // 方便报错时找到出错的位置 + GLogger glogger(argc, argv, "", ""); ros::init(argc, argv, "laserMapping"); ros::NodeHandle nh; image_transport::ImageTransport it(nh); - LIVMapper mapper(nh); - mapper.initializeSubscribersAndPublishers(nh, it); - mapper.run(); + LIVMapper mapper(nh,FLAGS_camera_config); + mapper.InitializeSubscribersAndPublishers(nh, it); + mapper.Run(); return 0; } \ No newline at end of file diff --git a/src/preprocess.cpp b/src/preprocess.cpp index afe34149..cd3cafdc 100755 --- a/src/preprocess.cpp +++ b/src/preprocess.cpp @@ -1,4 +1,4 @@ -/* +/* This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. Developer: Chunran Zheng @@ -12,124 +12,123 @@ which is included as part of this source code package. #include "preprocess.h" +#include "omp.h" + #define RETURN0 0x00 #define RETURN0AND1 0x10 -Preprocess::Preprocess() : feature_enabled(0), lidar_type(AVIA), blind(0.01), point_filter_num(1) -{ - inf_bound = 10; - N_SCANS = 6; - group_size = 8; - disA = 0.01; - disA = 0.1; // B? - p2l_ratio = 225; - limit_maxmid = 6.25; - limit_midmin = 6.25; - limit_maxmin = 3.24; - jump_up_limit = 170.0; - jump_down_limit = 8.0; - cos160 = 160.0; - edgea = 2; - edgeb = 0.1; - smallp_intersect = 172.5; - smallp_ratio = 1.2; - given_offset_time = false; - - jump_up_limit = cos(jump_up_limit / 180 * M_PI); - jump_down_limit = cos(jump_down_limit / 180 * M_PI); - cos160 = cos(cos160 / 180 * M_PI); - smallp_intersect = cos(smallp_intersect / 180 * M_PI); +Preprocess::Preprocess() + : feature_enabled_(0), lidar_type_(AVIA), blind_(0.01), point_filter_num_(1) { + inf_bound_ = 10; + n_scans_ = 6; + group_size_ = 8; + dis_a_ = 0.01; + dis_a_ = 0.1; // B? + p2l_ratio_ = 225; + limit_maxmid_ = 6.25; + limit_midmin_ = 6.25; + limit_maxmin_ = 3.24; + jump_up_limit_ = 170.0; + jump_down_limit_ = 8.0; + cos160_ = 160.0; + edge_a_ = 2; + edge_b_ = 0.1; + smallp_intersect_ = 172.5; + smallp_ratio_ = 1.2; + given_offset_time_ = false; + + jump_up_limit_ = cos(jump_up_limit_ / 180 * M_PI); + jump_down_limit_ = cos(jump_down_limit_ / 180 * M_PI); + cos160_ = cos(cos160_ / 180 * M_PI); + smallp_intersect_ = cos(smallp_intersect_ / 180 * M_PI); } Preprocess::~Preprocess() {} -void Preprocess::set(bool feat_en, int lid_type, double bld, int pfilt_num) -{ - feature_enabled = feat_en; - lidar_type = lid_type; - blind = bld; - point_filter_num = pfilt_num; +void Preprocess::Set(bool feat_en, int lid_type, double bld, int pfilt_num) { + feature_enabled_ = feat_en; + lidar_type_ = lid_type; + blind_ = bld; + point_filter_num_ = pfilt_num; } -void Preprocess::process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out) -{ - avia_handler(msg); - *pcl_out = pl_surf; +void Preprocess::Process(const livox_ros_driver::CustomMsg::ConstPtr &msg, + PointCloudXYZIN::Ptr &pcl_out) { + AviaHandler(msg); + *pcl_out = pl_surf_; } -void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out) -{ - switch (lidar_type) - { - case OUST64: - oust64_handler(msg); - break; - - case VELO16: - velodyne_handler(msg); - break; - - case L515: - l515_handler(msg); - break; - - case XT32: - xt32_handler(msg); - break; - - case PANDAR128: - Pandar128_handler(msg); - break; - - case ROBOSENSE: - robosense_handler(msg); - break; - - default: - printf("Error LiDAR Type: %d \n", lidar_type); - break; +void Preprocess::Process(const sensor_msgs::PointCloud2::ConstPtr &msg, + PointCloudXYZIN::Ptr &pcl_out) { + switch (lidar_type_) { + case OUST64: + Oust64Handler(msg); + break; + + case VELO16: + VelodyneHandler(msg); + break; + + case L515: + L515Handler(msg); + break; + + case XT32: + Xt32Handler(msg); + break; + + case PANDAR128: + Pandar128Handler(msg); + break; + + case ROBOSENSE: + RobosenseHandler(msg); + break; + + default: + printf("Error LiDAR Type: %d \n", lidar_type_); + break; } - *pcl_out = pl_surf; + *pcl_out = pl_surf_; } -void Preprocess::avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg) -{ - pl_surf.clear(); - pl_corn.clear(); - pl_full.clear(); +void Preprocess::AviaHandler( + const livox_ros_driver::CustomMsg::ConstPtr &msg) { + pl_surf_.clear(); + pl_corn_.clear(); + pl_full_.clear(); double t1 = omp_get_wtime(); int plsize = msg->point_num; printf("[ Preprocess ] Input point number: %d \n", plsize); // printf("point_filter_num: %d\n", point_filter_num); - pl_corn.reserve(plsize); - pl_surf.reserve(plsize); - pl_full.resize(plsize); + pl_corn_.reserve(plsize); + pl_surf_.reserve(plsize); + pl_full_.resize(plsize); - for (int i = 0; i < N_SCANS; i++) - { - pl_buff[i].clear(); - pl_buff[i].reserve(plsize); + for (int i = 0; i < n_scans_; i++) { + pl_buff_[i].clear(); + pl_buff_[i].reserve(plsize); } uint valid_num = 0; - if (feature_enabled) - { - for (uint i = 1; i < plsize; i++) - { - if ((msg->points[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10)) - { - pl_full[i].x = msg->points[i].x; - pl_full[i].y = msg->points[i].y; - pl_full[i].z = msg->points[i].z; - pl_full[i].intensity = msg->points[i].reflectivity; - pl_full[i].curvature = msg->points[i].offset_time / float(1000000); // use curvature as time of each laser points + if (feature_enabled_) { + for (uint i = 1; i < plsize; i++) { + if ((msg->points[i].line < n_scans_) && + ((msg->points[i].tag & 0x30) == 0x10)) { + pl_full_[i].x = msg->points[i].x; + pl_full_[i].y = msg->points[i].y; + pl_full_[i].z = msg->points[i].z; + pl_full_[i].intensity = msg->points[i].reflectivity; + pl_full_[i].curvature = + msg->points[i].offset_time / + float(1000000); // use curvature as time of each laser points bool is_new = false; - if ((abs(pl_full[i].x - pl_full[i - 1].x) > 1e-7) || (abs(pl_full[i].y - pl_full[i - 1].y) > 1e-7) || - (abs(pl_full[i].z - pl_full[i - 1].z) > 1e-7)) - { - pl_buff[msg->points[i].line].push_back(pl_full[i]); + if ((abs(pl_full_[i].x - pl_full_[i - 1].x) > 1e-7) || + (abs(pl_full_[i].y - pl_full_[i - 1].y) > 1e-7) || + (abs(pl_full_[i].z - pl_full_[i - 1].z) > 1e-7)) { + pl_buff_[msg->points[i].line].push_back(pl_full_[i]); } } } @@ -137,59 +136,65 @@ void Preprocess::avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg) static double time = 0.0; count++; double t0 = omp_get_wtime(); - for (int j = 0; j < N_SCANS; j++) - { - if (pl_buff[j].size() <= 5) continue; - pcl::PointCloud &pl = pl_buff[j]; + for (int j = 0; j < n_scans_; j++) { + if (pl_buff_[j].size() <= 5) continue; + pcl::PointCloud &pl = pl_buff_[j]; plsize = pl.size(); - vector &types = typess[j]; + std::vector &types = typess_[j]; types.clear(); types.resize(plsize); plsize--; - for (uint i = 0; i < plsize; i++) - { + for (uint i = 0; i < plsize; i++) { types[i].range = pl[i].x * pl[i].x + pl[i].y * pl[i].y; - vx = pl[i].x - pl[i + 1].x; - vy = pl[i].y - pl[i + 1].y; - vz = pl[i].z - pl[i + 1].z; - types[i].dista = vx * vx + vy * vy + vz * vz; + vx_ = pl[i].x - pl[i + 1].x; + vy_ = pl[i].y - pl[i + 1].y; + vz_ = pl[i].z - pl[i + 1].z; + types[i].dista = vx_ * vx_ + vy_ * vy_ + vz_ * vz_; } - types[plsize].range = pl[plsize].x * pl[plsize].x + pl[plsize].y * pl[plsize].y; - give_feature(pl, types); + types[plsize].range = + pl[plsize].x * pl[plsize].x + pl[plsize].y * pl[plsize].y; + GiveFeature(pl, types); // pl_surf += pl; } time += omp_get_wtime() - t0; printf("Feature extraction time: %lf \n", time / count); - } - else - { - for (uint i = 0; i < plsize; i++) - { - if ((msg->points[i].line < N_SCANS)) // && ((msg->points[i].tag & 0x30) == 0x10)) + } else { + for (uint i = 0; i < plsize; i++) { + if ((msg->points[i].line < + n_scans_)) // && ((msg->points[i].tag & 0x30) == 0x10)) { valid_num++; - pl_full[i].x = msg->points[i].x; - pl_full[i].y = msg->points[i].y; - pl_full[i].z = msg->points[i].z; - pl_full[i].intensity = msg->points[i].reflectivity; - pl_full[i].curvature = msg->points[i].offset_time / float(1000000); // use curvature as time of each laser points + pl_full_[i].x = msg->points[i].x; + pl_full_[i].y = msg->points[i].y; + pl_full_[i].z = msg->points[i].z; + pl_full_[i].intensity = msg->points[i].reflectivity; + pl_full_[i].curvature = + msg->points[i].offset_time / + float(1000000); // use curvature as time of each laser points + if (msg->points[i].offset_time > 1000000000) + std::cout << std::fixed + << "[ROSIO] point time :" << msg->points[i].offset_time; if (i == 0) - pl_full[i].curvature = fabs(pl_full[i].curvature) < 1.0 ? pl_full[i].curvature : 0.0; - else - { - // if(fabs(pl_full[i].curvature - pl_full[i - 1].curvature) > 1.0) ROS_ERROR("time jump: %f", fabs(pl_full[i].curvature - pl_full[i - 1].curvature)); - pl_full[i].curvature = fabs(pl_full[i].curvature - pl_full[i - 1].curvature) < 1.0 - ? pl_full[i].curvature - : pl_full[i - 1].curvature + 0.004166667f; // float(100/24000) + pl_full_[i].curvature = + fabs(pl_full_[i].curvature) < 1.0 ? pl_full_[i].curvature : 0.0; + else { + // if(fabs(pl_full[i].curvature - pl_full[i - 1].curvature) > 1.0) + // ROS_ERROR("time jump: %f", fabs(pl_full[i].curvature - pl_full[i - + // 1].curvature)); + pl_full_[i].curvature = + fabs(pl_full_[i].curvature - pl_full_[i - 1].curvature) < 1.0 + ? pl_full_[i].curvature + : pl_full_[i - 1].curvature + + 0.004166667f; // float(100/24000) } - if (valid_num % point_filter_num == 0) - { - if (pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y + pl_full[i].z * pl_full[i].z >= blind_sqr) - { - pl_surf.push_back(pl_full[i]); + if (valid_num % point_filter_num_ == 0) { + if (pl_full_[i].x * pl_full_[i].x + pl_full_[i].y * pl_full_[i].y + + pl_full_[i].z * pl_full_[i].z >= + blind_sqr_) { + pl_surf_.push_back(pl_full_[i]); // if (i % 100 == 0 || i == 0) printf("pl_full[i].curvature: %f \n", // pl_full[i].curvature); } @@ -197,33 +202,33 @@ void Preprocess::avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg) } } } - printf("[ Preprocess ] Output point number: %zu \n", pl_surf.points.size()); + printf("[ Preprocess ] Output point number: %zu \n", pl_surf_.points.size()); } -void Preprocess::l515_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) -{ - pl_surf.clear(); - pl_corn.clear(); - pl_full.clear(); +void Preprocess::L515Handler(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.size(); - pl_corn.reserve(plsize); - pl_surf.reserve(plsize); + pl_corn_.reserve(plsize); + pl_surf_.reserve(plsize); double time_stamp = msg->header.stamp.toSec(); // cout << "===================================" << endl; // printf("Pt size = %d, N_SCANS = %d\r\n", plsize, N_SCANS); - for (int i = 0; i < pl_orig.points.size(); i++) - { - if (i % point_filter_num != 0) continue; + for (int i = 0; i < pl_orig.points.size(); i++) { + if (i % point_filter_num_ != 0) continue; - double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + pl_orig.points[i].z * pl_orig.points[i].z; + double range = pl_orig.points[i].x * pl_orig.points[i].x + + pl_orig.points[i].y * pl_orig.points[i].y + + pl_orig.points[i].z * pl_orig.points[i].z; - if (range < blind_sqr) continue; + if (range < blind_sqr_) continue; Eigen::Vector3d pt_vec; - PointType added_pt; + PointXYZIN added_pt; added_pt.x = pl_orig.points[i].x; added_pt.y = pl_orig.points[i].y; added_pt.z = pl_orig.points[i].z; @@ -232,39 +237,36 @@ void Preprocess::l515_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) added_pt.normal_z = pl_orig.points[i].b; added_pt.curvature = 0.0; - pl_surf.points.push_back(added_pt); + pl_surf_.points.push_back(added_pt); } - cout << "pl size:: " << pl_orig.points.size() << endl; + std::cout << "pl size:: " << pl_orig.points.size() << std::endl; // pub_func(pl_surf, pub_full, msg->header.stamp); // pub_func(pl_surf, pub_corn, msg->header.stamp); } -void Preprocess::oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) -{ - pl_surf.clear(); - pl_corn.clear(); - pl_full.clear(); +void Preprocess::Oust64Handler(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.size(); - pl_corn.reserve(plsize); - pl_surf.reserve(plsize); - if (feature_enabled) - { - for (int i = 0; i < N_SCANS; i++) - { - pl_buff[i].clear(); - pl_buff[i].reserve(plsize); + pl_corn_.reserve(plsize); + pl_surf_.reserve(plsize); + if (feature_enabled_) { + for (int i = 0; i < n_scans_; i++) { + pl_buff_[i].clear(); + pl_buff_[i].reserve(plsize); } - for (uint i = 0; i < plsize; i++) - { - double range = - pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + pl_orig.points[i].z * pl_orig.points[i].z; - if (range < blind_sqr) continue; + for (uint i = 0; i < plsize; i++) { + double range = pl_orig.points[i].x * pl_orig.points[i].x + + pl_orig.points[i].y * pl_orig.points[i].y + + pl_orig.points[i].z * pl_orig.points[i].z; + if (range < blind_sqr_) continue; Eigen::Vector3d pt_vec; - PointType added_pt; + PointXYZIN added_pt; added_pt.x = pl_orig.points[i].x; added_pt.y = pl_orig.points[i].y; added_pt.z = pl_orig.points[i].z; @@ -277,45 +279,44 @@ void Preprocess::oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) if (yaw_angle <= -180.0) yaw_angle += 360.0; added_pt.curvature = pl_orig.points[i].t / 1e6; - if (pl_orig.points[i].ring < N_SCANS) { pl_buff[pl_orig.points[i].ring].push_back(added_pt); } + if (pl_orig.points[i].ring < n_scans_) { + pl_buff_[pl_orig.points[i].ring].push_back(added_pt); + } } - for (int j = 0; j < N_SCANS; j++) - { - PointCloudXYZI &pl = pl_buff[j]; + for (int j = 0; j < n_scans_; j++) { + PointCloudXYZIN &pl = pl_buff_[j]; int linesize = pl.size(); - vector &types = typess[j]; + std::vector &types = typess_[j]; types.clear(); types.resize(linesize); linesize--; - for (uint i = 0; i < linesize; i++) - { + for (uint i = 0; i < linesize; i++) { types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y); - vx = pl[i].x - pl[i + 1].x; - vy = pl[i].y - pl[i + 1].y; - vz = pl[i].z - pl[i + 1].z; - types[i].dista = vx * vx + vy * vy + vz * vz; + vx_ = pl[i].x - pl[i + 1].x; + vy_ = pl[i].y - pl[i + 1].y; + vz_ = pl[i].z - pl[i + 1].z; + types[i].dista = vx_ * vx_ + vy_ * vy_ + vz_ * vz_; } - types[linesize].range = sqrt(pl[linesize].x * pl[linesize].x + pl[linesize].y * pl[linesize].y); - give_feature(pl, types); + types[linesize].range = sqrt(pl[linesize].x * pl[linesize].x + + pl[linesize].y * pl[linesize].y); + GiveFeature(pl, types); } - } - else - { + } else { double time_stamp = msg->header.stamp.toSec(); // cout << "===================================" << endl; // printf("Pt size = %d, N_SCANS = %d\r\n", plsize, N_SCANS); - for (int i = 0; i < pl_orig.points.size(); i++) - { - if (i % point_filter_num != 0) continue; + for (int i = 0; i < pl_orig.points.size(); i++) { + if (i % point_filter_num_ != 0) continue; - double range = - pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + pl_orig.points[i].z * pl_orig.points[i].z; + double range = pl_orig.points[i].x * pl_orig.points[i].x + + pl_orig.points[i].y * pl_orig.points[i].y + + pl_orig.points[i].z * pl_orig.points[i].z; - if (range < blind_sqr) continue; + if (range < blind_sqr_) continue; Eigen::Vector3d pt_vec; - PointType added_pt; + PointXYZIN added_pt; added_pt.x = pl_orig.points[i].x; added_pt.y = pl_orig.points[i].y; added_pt.z = pl_orig.points[i].z; @@ -331,11 +332,12 @@ void Preprocess::oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) // cout<header.stamp); // pub_func(pl_surf, pub_corn, msg->header.stamp); @@ -343,69 +345,63 @@ void Preprocess::oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) #define MAX_LINE_NUM 64 -void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) -{ - pl_surf.clear(); - pl_corn.clear(); - pl_full.clear(); +void Preprocess::VelodyneHandler( + 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(); if (plsize == 0) return; - pl_surf.reserve(plsize); + pl_surf_.reserve(plsize); bool is_first[MAX_LINE_NUM]; - double yaw_fp[MAX_LINE_NUM] = {0}; // yaw of first scan point - double omega_l = 3.61; // scan angular velocity - float yaw_last[MAX_LINE_NUM] = {0.0}; // yaw of last scan point - float time_last[MAX_LINE_NUM] = {0.0}; // last offset time - - if (pl_orig.points[plsize - 1].time > 0) { given_offset_time = true; } - else - { - given_offset_time = false; + double yaw_fp[MAX_LINE_NUM] = {0}; // yaw of first scan point + double omega_l = 3.61; // scan angular velocity + float yaw_last[MAX_LINE_NUM] = {0.0}; // yaw of last scan point + float time_last[MAX_LINE_NUM] = {0.0}; // last offset time + + if (pl_orig.points[plsize - 1].time > 0) { + given_offset_time_ = true; + } else { + 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_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; - for (uint i = plsize - 1; i > 0; i--) - { - if (pl_orig.points[i].ring == layer_first) - { + 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; } } } - if (feature_enabled) - { - for (int i = 0; i < N_SCANS; i++) - { - pl_buff[i].clear(); - pl_buff[i].reserve(plsize); + if (feature_enabled_) { + for (int i = 0; i < n_scans_; i++) { + pl_buff_[i].clear(); + pl_buff_[i].reserve(plsize); } - for (int i = 0; i < plsize; i++) - { - PointType added_pt; + for (int i = 0; i < plsize; i++) { + PointXYZIN added_pt; added_pt.normal_x = 0; added_pt.normal_y = 0; added_pt.normal_z = 0; int layer = pl_orig.points[i].ring; - if (layer >= N_SCANS) continue; + if (layer >= n_scans_) continue; 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; - added_pt.curvature = pl_orig.points[i].time / 1000.0; // units: ms + added_pt.curvature = pl_orig.points[i].time / 1000.0; // units: ms - if (!given_offset_time) - { + if (!given_offset_time_) { double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957; - if (is_first[layer]) - { + if (is_first[layer]) { // printf("layer: %d; is first: %d", layer, is_first[layer]); yaw_fp[layer] = yaw_angle; is_first[layer] = false; @@ -415,44 +411,44 @@ void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) continue; } - 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 (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; + if (added_pt.curvature < time_last[layer]) + added_pt.curvature += 360.0 / omega_l; yaw_last[layer] = yaw_angle; time_last[layer] = added_pt.curvature; } - pl_buff[layer].points.push_back(added_pt); + pl_buff_[layer].points.push_back(added_pt); } - for (int j = 0; j < N_SCANS; j++) - { - PointCloudXYZI &pl = pl_buff[j]; + for (int j = 0; j < n_scans_; j++) { + PointCloudXYZIN &pl = pl_buff_[j]; int linesize = pl.size(); if (linesize < 2) continue; - vector &types = typess[j]; + std::vector &types = typess_[j]; types.clear(); types.resize(linesize); linesize--; - for (uint i = 0; i < linesize; i++) - { + for (uint i = 0; i < linesize; i++) { types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y); - vx = pl[i].x - pl[i + 1].x; - vy = pl[i].y - pl[i + 1].y; - vz = pl[i].z - pl[i + 1].z; - types[i].dista = vx * vx + vy * vy + vz * vz; + vx_ = pl[i].x - pl[i + 1].x; + vy_ = pl[i].y - pl[i + 1].y; + vz_ = pl[i].z - pl[i + 1].z; + types[i].dista = vx_ * vx_ + vy_ * vy_ + vz_ * vz_; } - types[linesize].range = sqrt(pl[linesize].x * pl[linesize].x + pl[linesize].y * pl[linesize].y); - give_feature(pl, types); + types[linesize].range = sqrt(pl[linesize].x * pl[linesize].x + + pl[linesize].y * pl[linesize].y); + GiveFeature(pl, types); } - } - else - { - for (int i = 0; i < plsize; i++) - { - PointType added_pt; + } else { + for (int i = 0; i < plsize; i++) { + PointXYZIN added_pt; // cout<<"!!!!!!"< blind_sqr) - { - pl_surf.points.push_back(added_pt); + 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_sqr_) { + pl_surf_.points.push_back(added_pt); // printf("time mode: %d time: %d \n", given_offset_time, // pl_orig.points[i].t); } @@ -511,19 +509,18 @@ void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) // pub_func(pl_surf, pub_corn, msg->header.stamp); } -void Preprocess::Pandar128_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) -{ - pl_surf.clear(); +void Preprocess::Pandar128Handler( + const sensor_msgs::PointCloud2::ConstPtr &msg) { + pl_surf_.clear(); pcl::PointCloud pl_orig; pcl::fromROSMsg(*msg, pl_orig); int plsize = pl_orig.points.size(); - pl_surf.reserve(plsize); + pl_surf_.reserve(plsize); double time_head = pl_orig.points[0].timestamp; - for (int i = 0; i < plsize; i++) - { - PointType added_pt; + for (int i = 0; i < plsize; i++) { + PointXYZIN added_pt; added_pt.normal_x = 0; added_pt.normal_y = 0; @@ -531,14 +528,15 @@ void Preprocess::Pandar128_handler(const sensor_msgs::PointCloud2::ConstPtr &msg 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 = static_cast(pl_orig.points[i].intensity) / 255.0f; + added_pt.intensity = + static_cast(pl_orig.points[i].intensity) / 255.0f; added_pt.curvature = (pl_orig.points[i].timestamp - time_head) * 1000.f; - 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_sqr) - { - pl_surf.points.push_back(added_pt); + 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_sqr_) { + pl_surf_.points.push_back(added_pt); // printf("time mode: %d time: %d \n", given_offset_time, // pl_orig.points[i].t); } @@ -546,52 +544,53 @@ void Preprocess::Pandar128_handler(const sensor_msgs::PointCloud2::ConstPtr &msg } // define a lambda function for the comparison - auto comparePoints = [](const PointType& a, const PointType& b) -> bool - { + auto comparePoints = [](const PointXYZIN &a, const PointXYZIN &b) -> bool { return a.curvature < b.curvature; }; - + // sort the points using the comparison function - std::sort(pl_surf.points.begin(), pl_surf.points.end(), comparePoints); - - // cout << GREEN << "pl_surf.points[0].timestamp: " << pl_surf.points[0].curvature << RESET << endl; - // cout << GREEN << "pl_surf.points[1000].timestamp: " << pl_surf.points[1000].curvature << RESET << endl; - // cout << GREEN << "pl_surf.points[5000].timestamp: " << pl_surf.points[5000].curvature << RESET << endl; - // cout << GREEN << "pl_surf.points[10000].timestamp: " << pl_surf.points[10000].curvature << RESET << endl; - // cout << GREEN << "pl_surf.points[20000].timestamp: " << pl_surf.points[20000].curvature << RESET << endl; - // cout << GREEN << "pl_surf.points[30000].timestamp: " << pl_surf.points[30000].curvature << RESET << endl; - // cout << GREEN << "pl_surf.points[31000].timestamp: " << pl_surf.points[31000].curvature << RESET << endl; + std::sort(pl_surf_.points.begin(), pl_surf_.points.end(), comparePoints); + + // cout << GREEN << "pl_surf.points[0].timestamp: " << + // pl_surf.points[0].curvature << RESET << endl; cout << GREEN << + // "pl_surf.points[1000].timestamp: " << pl_surf.points[1000].curvature << + // RESET << endl; cout << GREEN << "pl_surf.points[5000].timestamp: " << + // pl_surf.points[5000].curvature << RESET << endl; cout << GREEN << + // "pl_surf.points[10000].timestamp: " << pl_surf.points[10000].curvature << + // RESET << endl; cout << GREEN << "pl_surf.points[20000].timestamp: " << + // pl_surf.points[20000].curvature << RESET << endl; cout << GREEN << + // "pl_surf.points[30000].timestamp: " << pl_surf.points[30000].curvature << + // RESET << endl; cout << GREEN << "pl_surf.points[31000].timestamp: " << + // pl_surf.points[31000].curvature << RESET << endl; } -void Preprocess::xt32_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) -{ - pl_surf.clear(); - pl_corn.clear(); - pl_full.clear(); +void Preprocess::Xt32Handler(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(); - pl_surf.reserve(plsize); + pl_surf_.reserve(plsize); bool is_first[MAX_LINE_NUM]; - double yaw_fp[MAX_LINE_NUM] = {0}; // yaw of first scan point - double omega_l = 3.61; // scan angular velocity - float yaw_last[MAX_LINE_NUM] = {0.0}; // yaw of last scan point - float time_last[MAX_LINE_NUM] = {0.0}; // last offset time - - if (pl_orig.points[plsize - 1].timestamp > 0) { given_offset_time = true; } - else - { - given_offset_time = false; + double yaw_fp[MAX_LINE_NUM] = {0}; // yaw of first scan point + double omega_l = 3.61; // scan angular velocity + float yaw_last[MAX_LINE_NUM] = {0.0}; // yaw of last scan point + float time_last[MAX_LINE_NUM] = {0.0}; // last offset time + + if (pl_orig.points[plsize - 1].timestamp > 0) { + given_offset_time_ = true; + } else { + 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_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; - for (uint i = plsize - 1; i > 0; i--) - { - if (pl_orig.points[i].ring == layer_first) - { + 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; } @@ -600,33 +599,28 @@ void Preprocess::xt32_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) double time_head = pl_orig.points[0].timestamp; - if (feature_enabled) - { - for (int i = 0; i < N_SCANS; i++) - { - pl_buff[i].clear(); - pl_buff[i].reserve(plsize); + if (feature_enabled_) { + for (int i = 0; i < n_scans_; i++) { + pl_buff_[i].clear(); + pl_buff_[i].reserve(plsize); } - for (int i = 0; i < plsize; i++) - { - PointType added_pt; + for (int i = 0; i < plsize; i++) { + PointXYZIN added_pt; added_pt.normal_x = 0; added_pt.normal_y = 0; added_pt.normal_z = 0; int layer = pl_orig.points[i].ring; - if (layer >= N_SCANS) continue; + if (layer >= n_scans_) continue; 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; - added_pt.curvature = pl_orig.points[i].timestamp / 1000.0; // units: ms + added_pt.curvature = pl_orig.points[i].timestamp / 1000.0; // units: ms - if (!given_offset_time) - { + if (!given_offset_time_) { double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957; - if (is_first[layer]) - { + if (is_first[layer]) { // printf("layer: %d; is first: %d", layer, is_first[layer]); yaw_fp[layer] = yaw_angle; is_first[layer] = false; @@ -636,44 +630,44 @@ void Preprocess::xt32_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) continue; } - 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 (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; + if (added_pt.curvature < time_last[layer]) + added_pt.curvature += 360.0 / omega_l; yaw_last[layer] = yaw_angle; time_last[layer] = added_pt.curvature; } - pl_buff[layer].points.push_back(added_pt); + pl_buff_[layer].points.push_back(added_pt); } - for (int j = 0; j < N_SCANS; j++) - { - PointCloudXYZI &pl = pl_buff[j]; + for (int j = 0; j < n_scans_; j++) { + PointCloudXYZIN &pl = pl_buff_[j]; int linesize = pl.size(); if (linesize < 2) continue; - vector &types = typess[j]; + std::vector &types = typess_[j]; types.clear(); types.resize(linesize); linesize--; - for (uint i = 0; i < linesize; i++) - { + for (uint i = 0; i < linesize; i++) { types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y); - vx = pl[i].x - pl[i + 1].x; - vy = pl[i].y - pl[i + 1].y; - vz = pl[i].z - pl[i + 1].z; - types[i].dista = vx * vx + vy * vy + vz * vz; + vx_ = pl[i].x - pl[i + 1].x; + vy_ = pl[i].y - pl[i + 1].y; + vz_ = pl[i].z - pl[i + 1].z; + types[i].dista = vx_ * vx_ + vy_ * vy_ + vz_ * vz_; } - types[linesize].range = sqrt(pl[linesize].x * pl[linesize].x + pl[linesize].y * pl[linesize].y); - give_feature(pl, types); + types[linesize].range = sqrt(pl[linesize].x * pl[linesize].x + + pl[linesize].y * pl[linesize].y); + GiveFeature(pl, types); } - } - else - { - for (int i = 0; i < plsize; i++) - { - PointType added_pt; + } else { + for (int i = 0; i < plsize; i++) { + PointXYZIN added_pt; // cout<<"!!!!!!"< blind_sqr) - { - pl_surf.points.push_back(added_pt); + 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_sqr_) { + pl_surf_.points.push_back(added_pt); // printf("time mode: %d time: %d \n", given_offset_time, // pl_orig.points[i].t); } @@ -707,27 +701,27 @@ void Preprocess::xt32_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) // pub_func(pl_surf, pub_corn, msg->header.stamp); } -void Preprocess::robosense_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) -{ - pl_surf.clear(); +void Preprocess::RobosenseHandler( + const sensor_msgs::PointCloud2::ConstPtr &msg) { + pl_surf_.clear(); pcl::PointCloud pl_orig; pcl::fromROSMsg(*msg, pl_orig); int plsize = pl_orig.size(); - pl_surf.reserve(plsize); + pl_surf_.reserve(plsize); double time_head = pl_orig.points[0].timestamp; - for (int i = 0; i < plsize; ++i) - { - if (i % point_filter_num != 0) continue; + for (int i = 0; i < plsize; ++i) { + if (i % point_filter_num_ != 0) continue; - const auto& pt = pl_orig.points[i]; + const auto &pt = pl_orig.points[i]; const double x = pt.x, y = pt.y, z = pt.z; const double dist_sqr = x * x + y * y + z * z; - const bool is_valid = (dist_sqr >= blind_sqr) && !std::isnan(x) && !std::isnan(y) && !std::isnan(z); + const bool is_valid = (dist_sqr >= blind_sqr_) && !std::isnan(x) && + !std::isnan(y) && !std::isnan(z); if (!is_valid) continue; - PointType added_pt; + PointXYZIN added_pt; added_pt.normal_x = 0; added_pt.normal_y = 0; added_pt.normal_z = 0; @@ -736,31 +730,30 @@ void Preprocess::robosense_handler(const sensor_msgs::PointCloud2::ConstPtr &msg added_pt.z = pt.z; added_pt.intensity = pt.intensity; added_pt.curvature = (pt.timestamp - time_head) * 1000.0; - pl_surf.points.push_back(added_pt); + pl_surf_.points.push_back(added_pt); } - std::sort(pl_surf.points.begin(), pl_surf.points.end(), [](const PointType &a, const PointType &b) { - return a.curvature < b.curvature; - }); + std::sort(pl_surf_.points.begin(), pl_surf_.points.end(), + [](const PointXYZIN &a, const PointXYZIN &b) { + return a.curvature < b.curvature; + }); } -void Preprocess::give_feature(pcl::PointCloud &pl, vector &types) -{ +void Preprocess::GiveFeature(pcl::PointCloud &pl, + std::vector &types) { int plsize = pl.size(); int plsize2; - if (plsize == 0) - { + if (plsize == 0) { printf("something wrong\n"); return; } uint head = 0; - while (types[head].range < blind_sqr) - { + while (types[head].range < blind_sqr_) { head++; } // Surf - plsize2 = (plsize > group_size) ? (plsize - group_size) : 0; + plsize2 = (plsize > group_size_) ? (plsize - group_size_) : 0; Eigen::Vector3d curr_direct(Eigen::Vector3d::Zero()); Eigen::Vector3d last_direct(Eigen::Vector3d::Zero()); @@ -771,34 +764,37 @@ void Preprocess::give_feature(pcl::PointCloud &pl, vector &t int last_state = 0; int plane_type; - for (uint i = head; i < plsize2; i++) - { - if (types[i].range < blind_sqr) { continue; } + for (uint i = head; i < plsize2; i++) { + if (types[i].range < blind_sqr_) { + continue; + } i2 = i; - plane_type = plane_judge(pl, types, i, i_nex, curr_direct); + plane_type = PlaneJudge(pl, types, i, i_nex, curr_direct); - if (plane_type == 1) - { - for (uint j = i; j <= i_nex; j++) - { - if (j != i && j != i_nex) { types[j].ftype = Real_Plane; } - else { types[j].ftype = Poss_Plane; } + if (plane_type == 1) { + for (uint j = i; j <= i_nex; j++) { + if (j != i && j != i_nex) { + types[j].ftype = Real_Plane; + } else { + types[j].ftype = Poss_Plane; + } } // if(last_state==1 && fabs(last_direct.sum())>0.5) - if (last_state == 1 && last_direct.norm() > 0.1) - { + if (last_state == 1 && last_direct.norm() > 0.1) { double mod = last_direct.transpose() * curr_direct; - if (mod > -0.707 && mod < 0.707) { types[i].ftype = Edge_Plane; } - else { types[i].ftype = Real_Plane; } + if (mod > -0.707 && mod < 0.707) { + types[i].ftype = Edge_Plane; + } else { + types[i].ftype = Real_Plane; + } } i = i_nex - 1; last_state = 1; - } - else // if(plane_type == 2) + } else // if(plane_type == 2) { i = i_nex; last_state = 0; @@ -856,24 +852,30 @@ void Preprocess::give_feature(pcl::PointCloud &pl, vector &t } plsize2 = plsize > 3 ? plsize - 3 : 0; - for (uint i = head + 3; i < plsize2; i++) - { - if (types[i].range < blind_sqr || types[i].ftype >= Real_Plane) { continue; } + for (uint i = head + 3; i < plsize2; i++) { + if (types[i].range < blind_sqr_ || types[i].ftype >= Real_Plane) { + continue; + } - if (types[i - 1].dista < 1e-16 || types[i].dista < 1e-16) { continue; } + if (types[i - 1].dista < 1e-16 || types[i].dista < 1e-16) { + continue; + } Eigen::Vector3d vec_a(pl[i].x, pl[i].y, pl[i].z); Eigen::Vector3d vecs[2]; - for (int j = 0; j < 2; j++) - { + for (int j = 0; j < 2; j++) { int m = -1; - if (j == 1) { m = 1; } + if (j == 1) { + m = 1; + } - if (types[i + m].range < blind_sqr) - { - if (types[i].range > inf_bound) { types[i].edj[j] = Nr_inf; } - else { types[i].edj[j] = Nr_blind; } + if (types[i + m].range < blind_sqr_) { + if (types[i].range > inf_bound_) { + types[i].edj[j] = Nr_inf; + } else { + types[i].edj[j] = Nr_blind; + } continue; } @@ -881,88 +883,100 @@ void Preprocess::give_feature(pcl::PointCloud &pl, vector &t vecs[j] = vecs[j] - vec_a; types[i].angle[j] = vec_a.dot(vecs[j]) / vec_a.norm() / vecs[j].norm(); - if (types[i].angle[j] < jump_up_limit) { types[i].edj[j] = Nr_180; } - else if (types[i].angle[j] > jump_down_limit) { types[i].edj[j] = Nr_zero; } + if (types[i].angle[j] < jump_up_limit_) { + types[i].edj[j] = Nr_180; + } else if (types[i].angle[j] > jump_down_limit_) { + types[i].edj[j] = Nr_zero; + } } - types[i].intersect = vecs[Prev].dot(vecs[Next]) / vecs[Prev].norm() / vecs[Next].norm(); - if (types[i].edj[Prev] == Nr_nor && types[i].edj[Next] == Nr_zero && types[i].dista > 0.0225 && types[i].dista > 4 * types[i - 1].dista) - { - if (types[i].intersect > cos160) - { - if (edge_jump_judge(pl, types, i, Prev)) { types[i].ftype = Edge_Jump; } + types[i].intersect = + vecs[Prev].dot(vecs[Next]) / vecs[Prev].norm() / vecs[Next].norm(); + if (types[i].edj[Prev] == Nr_nor && types[i].edj[Next] == Nr_zero && + types[i].dista > 0.0225 && types[i].dista > 4 * types[i - 1].dista) { + if (types[i].intersect > cos160_) { + if (EdgeJumpJudge(pl, types, i, Prev)) { + types[i].ftype = Edge_Jump; + } } - } - else if (types[i].edj[Prev] == Nr_zero && types[i].edj[Next] == Nr_nor && types[i - 1].dista > 0.0225 && types[i - 1].dista > 4 * types[i].dista) - { - if (types[i].intersect > cos160) - { - if (edge_jump_judge(pl, types, i, Next)) { types[i].ftype = Edge_Jump; } + } else if (types[i].edj[Prev] == Nr_zero && types[i].edj[Next] == Nr_nor && + types[i - 1].dista > 0.0225 && + types[i - 1].dista > 4 * types[i].dista) { + if (types[i].intersect > cos160_) { + if (EdgeJumpJudge(pl, types, i, Next)) { + types[i].ftype = Edge_Jump; + } + } + } else if (types[i].edj[Prev] == Nr_nor && types[i].edj[Next] == Nr_inf) { + if (EdgeJumpJudge(pl, types, i, Prev)) { + types[i].ftype = Edge_Jump; + } + } else if (types[i].edj[Prev] == Nr_inf && types[i].edj[Next] == Nr_nor) { + if (EdgeJumpJudge(pl, types, i, Next)) { + types[i].ftype = Edge_Jump; + } + } else if (types[i].edj[Prev] > Nr_nor && types[i].edj[Next] > Nr_nor) { + if (types[i].ftype == Nor) { + types[i].ftype = Wire; } - } - else if (types[i].edj[Prev] == Nr_nor && types[i].edj[Next] == Nr_inf) - { - if (edge_jump_judge(pl, types, i, Prev)) { types[i].ftype = Edge_Jump; } - } - else if (types[i].edj[Prev] == Nr_inf && types[i].edj[Next] == Nr_nor) - { - if (edge_jump_judge(pl, types, i, Next)) { types[i].ftype = Edge_Jump; } - } - else if (types[i].edj[Prev] > Nr_nor && types[i].edj[Next] > Nr_nor) - { - if (types[i].ftype == Nor) { types[i].ftype = Wire; } } } plsize2 = plsize - 1; double ratio; - for (uint i = head + 1; i < plsize2; i++) - { - if (types[i].range < blind_sqr || types[i - 1].range < blind_sqr || types[i + 1].range < blind_sqr) { continue; } + for (uint i = head + 1; i < plsize2; i++) { + if (types[i].range < blind_sqr_ || types[i - 1].range < blind_sqr_ || + types[i + 1].range < blind_sqr_) { + continue; + } - if (types[i - 1].dista < 1e-8 || types[i].dista < 1e-8) { continue; } + if (types[i - 1].dista < 1e-8 || types[i].dista < 1e-8) { + continue; + } - if (types[i].ftype == Nor) - { - if (types[i - 1].dista > types[i].dista) { ratio = types[i - 1].dista / types[i].dista; } - else { ratio = types[i].dista / types[i - 1].dista; } + if (types[i].ftype == Nor) { + if (types[i - 1].dista > types[i].dista) { + ratio = types[i - 1].dista / types[i].dista; + } else { + ratio = types[i].dista / types[i - 1].dista; + } - if (types[i].intersect < smallp_intersect && ratio < smallp_ratio) - { - if (types[i - 1].ftype == Nor) { types[i - 1].ftype = Real_Plane; } - if (types[i + 1].ftype == Nor) { types[i + 1].ftype = Real_Plane; } + if (types[i].intersect < smallp_intersect_ && ratio < smallp_ratio_) { + if (types[i - 1].ftype == Nor) { + types[i - 1].ftype = Real_Plane; + } + if (types[i + 1].ftype == Nor) { + types[i + 1].ftype = Real_Plane; + } types[i].ftype = Real_Plane; } } } int last_surface = -1; - for (uint j = head; j < plsize; j++) - { - if (types[j].ftype == Poss_Plane || types[j].ftype == Real_Plane) - { - if (last_surface == -1) { last_surface = j; } + for (uint j = head; j < plsize; j++) { + if (types[j].ftype == Poss_Plane || types[j].ftype == Real_Plane) { + if (last_surface == -1) { + last_surface = j; + } - if (j == uint(last_surface + point_filter_num - 1)) - { - PointType ap; + if (j == uint(last_surface + point_filter_num_ - 1)) { + PointXYZIN ap; ap.x = pl[j].x; ap.y = pl[j].y; ap.z = pl[j].z; ap.curvature = pl[j].curvature; - pl_surf.push_back(ap); + pl_surf_.push_back(ap); last_surface = -1; } - } - else - { - if (types[j].ftype == Edge_Jump || types[j].ftype == Edge_Plane) { pl_corn.push_back(pl[j]); } - if (last_surface != -1) - { - PointType ap; - for (uint k = last_surface; k < j; k++) - { + } else { + if (types[j].ftype == Edge_Jump || types[j].ftype == Edge_Plane) { + pl_corn_.push_back(pl[j]); + } + if (last_surface != -1) { + PointXYZIN ap; + for (uint k = last_surface; k < j; k++) { ap.x += pl[k].x; ap.y += pl[k].y; ap.z += pl[k].z; @@ -972,15 +986,14 @@ void Preprocess::give_feature(pcl::PointCloud &pl, vector &t ap.y /= (j - last_surface); ap.z /= (j - last_surface); ap.curvature /= (j - last_surface); - pl_surf.push_back(ap); + pl_surf_.push_back(ap); } last_surface = -1; } } } -void Preprocess::pub_func(PointCloudXYZI &pl, const ros::Time &ct) -{ +void Preprocess::PubFunc(PointCloudXYZIN &pl, const ros::Time &ct) { pl.height = 1; pl.width = pl.size(); sensor_msgs::PointCloud2 output; @@ -989,74 +1002,70 @@ void Preprocess::pub_func(PointCloudXYZI &pl, const ros::Time &ct) output.header.stamp = ct; } -int Preprocess::plane_judge(const PointCloudXYZI &pl, vector &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct) -{ - double group_dis = disA * types[i_cur].range + disB; +int Preprocess::PlaneJudge(const PointCloudXYZIN &pl, std::vector &types, + uint i_cur, uint &i_nex, + Eigen::Vector3d &curr_direct) { + double group_dis = dis_a_ * types[i_cur].range + dis_b_; group_dis = group_dis * group_dis; // i_nex = i_cur; double two_dis; - vector disarr; + std::vector disarr; disarr.reserve(20); - for (i_nex = i_cur; i_nex < i_cur + group_size; i_nex++) - { - if (types[i_nex].range < blind_sqr) - { + for (i_nex = i_cur; i_nex < i_cur + group_size_; i_nex++) { + if (types[i_nex].range < blind_sqr_) { curr_direct.setZero(); return 2; } disarr.push_back(types[i_nex].dista); } - for (;;) - { + for (;;) { if ((i_cur >= pl.size()) || (i_nex >= pl.size())) break; - if (types[i_nex].range < blind_sqr) - { + if (types[i_nex].range < blind_sqr_) { curr_direct.setZero(); return 2; } - vx = pl[i_nex].x - pl[i_cur].x; - vy = pl[i_nex].y - pl[i_cur].y; - vz = pl[i_nex].z - pl[i_cur].z; - two_dis = vx * vx + vy * vy + vz * vz; - if (two_dis >= group_dis) { break; } + vx_ = pl[i_nex].x - pl[i_cur].x; + vy_ = pl[i_nex].y - pl[i_cur].y; + vz_ = pl[i_nex].z - pl[i_cur].z; + two_dis = vx_ * vx_ + vy_ * vy_ + vz_ * vz_; + if (two_dis >= group_dis) { + break; + } disarr.push_back(types[i_nex].dista); i_nex++; } double leng_wid = 0; double v1[3], v2[3]; - for (uint j = i_cur + 1; j < i_nex; j++) - { + for (uint j = i_cur + 1; j < i_nex; j++) { if ((j >= pl.size()) || (i_cur >= pl.size())) break; v1[0] = pl[j].x - pl[i_cur].x; v1[1] = pl[j].y - pl[i_cur].y; v1[2] = pl[j].z - pl[i_cur].z; - v2[0] = v1[1] * vz - vy * v1[2]; - v2[1] = v1[2] * vx - v1[0] * vz; - v2[2] = v1[0] * vy - vx * v1[1]; + v2[0] = v1[1] * vz_ - vy_ * v1[2]; + v2[1] = v1[2] * vx_ - v1[0] * vz_; + v2[2] = v1[0] * vy_ - vx_ * v1[1]; double lw = v2[0] * v2[0] + v2[1] * v2[1] + v2[2] * v2[2]; - if (lw > leng_wid) { leng_wid = lw; } + if (lw > leng_wid) { + leng_wid = lw; + } } - if ((two_dis * two_dis / leng_wid) < p2l_ratio) - { + if ((two_dis * two_dis / leng_wid) < p2l_ratio_) { curr_direct.setZero(); return 0; } uint disarrsize = disarr.size(); - for (uint j = 0; j < disarrsize - 1; j++) - { - for (uint k = j + 1; k < disarrsize; k++) - { - if (disarr[j] < disarr[k]) - { + for (uint j = 0; j < disarrsize - 1; j++) { + for (uint k = j + 1; k < disarrsize; k++) { + if (disarr[j] < disarr[k]) { leng_wid = disarr[j]; disarr[j] = disarr[k]; disarr[k] = leng_wid; @@ -1064,54 +1073,49 @@ int Preprocess::plane_judge(const PointCloudXYZI &pl, vector &types, ui } } - if (disarr[disarr.size() - 2] < 1e-16) - { + if (disarr[disarr.size() - 2] < 1e-16) { curr_direct.setZero(); return 0; } - if (lidar_type == AVIA) - { + if (lidar_type_ == AVIA) { double dismax_mid = disarr[0] / disarr[disarrsize / 2]; double dismid_min = disarr[disarrsize / 2] / disarr[disarrsize - 2]; - if (dismax_mid >= limit_maxmid || dismid_min >= limit_midmin) - { + if (dismax_mid >= limit_maxmid_ || dismid_min >= limit_midmin_) { curr_direct.setZero(); return 0; } - } - else - { + } else { double dismax_min = disarr[0] / disarr[disarrsize - 2]; - if (dismax_min >= limit_maxmin) - { + if (dismax_min >= limit_maxmin_) { curr_direct.setZero(); return 0; } } - curr_direct << vx, vy, vz; + curr_direct << vx_, vy_, vz_; curr_direct.normalize(); return 1; } -bool Preprocess::edge_jump_judge(const PointCloudXYZI &pl, vector &types, uint i, Surround nor_dir) -{ - if (nor_dir == 0) - { - if (types[i - 1].range < blind_sqr || types[i - 2].range < blind_sqr) { return false; } - } - else if (nor_dir == 1) - { - if (types[i + 1].range < blind_sqr || types[i + 2].range < blind_sqr) { return false; } +bool Preprocess::EdgeJumpJudge(const PointCloudXYZIN &pl, + std::vector &types, uint i, + Surround nor_dir) { + if (nor_dir == 0) { + if (types[i - 1].range < blind_sqr_ || types[i - 2].range < blind_sqr_) { + return false; + } + } else if (nor_dir == 1) { + if (types[i + 1].range < blind_sqr_ || types[i + 2].range < blind_sqr_) { + return false; + } } double d1 = types[i + nor_dir - 1].dista; double d2 = types[i + 3 * nor_dir - 2].dista; double d; - if (d1 < d2) - { + if (d1 < d2) { d = d1; d1 = d2; d2 = d; @@ -1120,7 +1124,9 @@ bool Preprocess::edge_jump_judge(const PointCloudXYZI &pl, vector &type d1 = sqrt(d1); d2 = sqrt(d2); - if (d1 > edgea * d2 || (d1 - d2) > edgeb) { return false; } + if (d1 > edge_a_ * d2 || (d1 - d2) > edge_b_) { + return false; + } return true; } \ No newline at end of file diff --git a/src/vio.cpp b/src/vio.cpp index 2ff8fd0f..87b7a8d9 100755 --- a/src/vio.cpp +++ b/src/vio.cpp @@ -1,4 +1,4 @@ -/* +/* This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. Developer: Chunran Zheng @@ -12,196 +12,198 @@ which is included as part of this source code package. #include "vio.h" -VIOManager::VIOManager() -{ +#include + +VIOManager::VIOManager() { // downSizeFilter.setLeafSize(0.2, 0.2, 0.2); } -VIOManager::~VIOManager() -{ - delete visual_submap; - for (auto& pair : warp_map) delete pair.second; - warp_map.clear(); - for (auto& pair : feat_map) delete pair.second; - feat_map.clear(); +VIOManager::~VIOManager() { + delete visual_submap_; + for (auto &pair : warp_map_) delete pair.second; + warp_map_.clear(); + for (auto &pair : vp_map_) delete pair.second->second; + vp_map_.clear(); + // for (auto &pair : feat_map_) delete pair.second; + // feat_map_.clear(); + vp_data_.clear(); } -void VIOManager::setImuToLidarExtrinsic(const V3D &transl, const M3D &rot) -{ - Pli = -rot.transpose() * transl; - Rli = rot.transpose(); +void VIOManager::SetImuToLidarExtrinsic(const V3D &transl, const M3D &rot) { + Pli_ = -rot.transpose() * transl; + Rli_ = rot.transpose(); + se3_imu_lidar_ = SE3(Mat3ToSO3(Rli_), Pli_); } -void VIOManager::setLidarToCameraExtrinsic(vector &R, vector &P) -{ - Rcl << MAT_FROM_ARRAY(R); - Pcl << VEC_FROM_ARRAY(P); +void VIOManager::SetLidarToCameraExtrinsic(std::vector &R, + std::vector &P) { + Rcl_ << MAT_FROM_ARRAY(R); + Pcl_ << VEC_FROM_ARRAY(P); + se3_lidar_cam_ = SE3(Mat3ToSO3(Rcl_), Pcl_); } -void VIOManager::initializeVIO() -{ - visual_submap = new SubSparseMap; +void VIOManager::InitializeVIO() { + visual_submap_ = new SubSparseMap; - fx = cam->fx(); - fy = cam->fy(); - cx = cam->cx(); - cy = cam->cy(); - image_resize_factor = cam->scale(); + fx_ = cam_->fx(); + fy_ = cam_->fy(); + cx_ = cam_->cx(); + cy_ = cam_->cy(); + image_resize_factor_ = cam_->scale(); - printf("intrinsic: %.6lf, %.6lf, %.6lf, %.6lf\n", fx, fy, cx, cy); + printf("intrinsic: %.6lf, %.6lf, %.6lf, %.6lf\n", fx_, fy_, cx_, cy_); - width = cam->width(); - height = cam->height(); + width_ = cam_->width(); + height_ = cam_->height(); - printf("width: %d, height: %d, scale: %f\n", width, height, image_resize_factor); - Rci = Rcl * Rli; - Pci = Rcl * Pli + Pcl; + printf("width: %d, height: %d, scale: %f\n", width_, height_, + image_resize_factor_); + Rci_ = Rcl_ * Rli_; + Pci_ = Rcl_ * Pli_ + Pcl_; V3D Pic; M3D tmp; - Jdphi_dR = Rci; - Pic = -Rci.transpose() * Pci; + Jdphi_dR_ = Rci_; + Pic = -Rci_.transpose() * Pci_; tmp << SKEW_SYM_MATRX(Pic); - Jdp_dR = -Rci * tmp; - - if (grid_size > 10) - { - grid_n_width = ceil(static_cast(width / grid_size)); - grid_n_height = ceil(static_cast(height / grid_size)); + Jdp_dR_ = -Rci_ * tmp; + + if (grid_size_ > 10) { + grid_n_width_ = ceil(static_cast(width_ / grid_size_)); + grid_n_height_ = ceil(static_cast(height_ / grid_size_)); + } else { + grid_size_ = static_cast(height_ / grid_n_height_); + grid_n_height_ = ceil(static_cast(height_ / grid_size_)); + grid_n_width_ = ceil(static_cast(width_ / grid_size_)); } - else - { - grid_size = static_cast(height / grid_n_height); - grid_n_height = ceil(static_cast(height / grid_size)); - grid_n_width = ceil(static_cast(width / grid_size)); - } - length = grid_n_width * grid_n_height; + length_ = grid_n_width_ * grid_n_height_; - if(raycast_en) - { + if (raycast_en_) { // cv::Mat img_test = cv::Mat::zeros(height, width, CV_8UC1); // uchar* it = (uchar*)img_test.data; - border_flag.resize(length, 0); + border_flag_.resize(length_, 0); - std::vector>().swap(rays_with_sample_points); - rays_with_sample_points.reserve(length); - printf("grid_size: %d, grid_n_height: %d, grid_n_width: %d, length: %d\n", grid_size, grid_n_height, grid_n_width, length); + std::vector>().swap(rays_with_sample_points_); + rays_with_sample_points_.reserve(length_); + printf("grid_size: %d, grid_n_height: %d, grid_n_width: %d, length: %d\n", + grid_size_, grid_n_height_, grid_n_width_, length_); float d_min = 0.1; float d_max = 3.0; float step = 0.2; - for (int grid_row = 1; grid_row <= grid_n_height; grid_row++) - { - for (int grid_col = 1; grid_col <= grid_n_width; grid_col++) - { + for (int grid_row = 1; grid_row <= grid_n_height_; grid_row++) { + for (int grid_col = 1; grid_col <= grid_n_width_; grid_col++) { std::vector SamplePointsEachGrid; - int index = (grid_row - 1) * grid_n_width + grid_col - 1; + int index = (grid_row - 1) * grid_n_width_ + grid_col - 1; - if (grid_row == 1 || grid_col == 1 || grid_row == grid_n_height || grid_col == grid_n_width) border_flag[index] = 1; + if (grid_row == 1 || grid_col == 1 || grid_row == grid_n_height_ || + grid_col == grid_n_width_) + border_flag_[index] = 1; - int u = grid_size / 2 + (grid_col - 1) * grid_size; - int v = grid_size / 2 + (grid_row - 1) * grid_size; + int u = grid_size_ / 2 + (grid_col - 1) * grid_size_; + int v = grid_size_ / 2 + (grid_row - 1) * grid_size_; // it[ u + v * width ] = 255; - for (float d_temp = d_min; d_temp <= d_max; d_temp += step) - { + for (float d_temp = d_min; d_temp <= d_max; d_temp += step) { V3D xyz; - xyz = cam->cam2world(u, v); + xyz = cam_->cam2world(u, v); xyz *= d_temp / xyz[2]; // xyz[0] = (u - cx) / fx * d_temp; // xyz[1] = (v - cy) / fy * d_temp; // xyz[2] = d_temp; SamplePointsEachGrid.push_back(xyz); } - rays_with_sample_points.push_back(SamplePointsEachGrid); + rays_with_sample_points_.push_back(SamplePointsEachGrid); } } // printf("rays_with_sample_points: %d, RaysWithSamplePointsCapacity: %d, - // rays_with_sample_points[0].capacity(): %d, rays_with_sample_points[0]: %d\n", - // rays_with_sample_points.size(), rays_with_sample_points.capacity(), - // rays_with_sample_points[0].capacity(), rays_with_sample_points[0].size()); for - // (const auto & it : rays_with_sample_points[0]) cout << it.transpose() << endl; + // rays_with_sample_points[0].capacity(): %d, rays_with_sample_points[0]: + // %d\n", rays_with_sample_points.size(), + // rays_with_sample_points.capacity(), + // rays_with_sample_points[0].capacity(), + // rays_with_sample_points[0].size()); for (const auto & it : + // rays_with_sample_points[0]) cout << it.transpose() << endl; // cv::imshow("img_test", img_test); // cv::waitKey(1); } - if(colmap_output_en) - { - pinhole_cam = dynamic_cast(cam); - fout_colmap.open(DEBUG_FILE_DIR("Colmap/sparse/0/images.txt"), ios::out); - fout_colmap << "# Image list with two lines of data per image:\n"; - fout_colmap << "# IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME\n"; - fout_colmap << "# POINTS2D[] as (X, Y, POINT3D_ID)\n"; - fout_camera.open(DEBUG_FILE_DIR("Colmap/sparse/0/cameras.txt"), ios::out); - fout_camera << "# Camera list with one line of data per camera:\n"; - fout_camera << "# CAMERA_ID, MODEL, WIDTH, HEIGHT, PARAMS[]\n"; - fout_camera << "1 PINHOLE " << width << " " << height << " " - << std::fixed << std::setprecision(6) // 控制浮点数精度为10位 - << fx << " " << fy << " " - << cx << " " << cy << std::endl; - fout_camera.close(); + if (colmap_output_en_) { + pinhole_cam_ = dynamic_cast(cam_); + fout_colmap_.open(DEBUG_FILE_DIR("Colmap/sparse/0/images.txt"), + std::ios::out); + fout_colmap_ << "# Image list with two lines of data per image:\n"; + fout_colmap_ + << "# IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME\n"; + fout_colmap_ << "# POINTS2D[] as (X, Y, POINT3D_ID)\n"; + fout_camera_.open(DEBUG_FILE_DIR("Colmap/sparse/0/cameras.txt"), + std::ios::out); + fout_camera_ << "# Camera list with one line of data per camera:\n"; + fout_camera_ << "# CAMERA_ID, MODEL, WIDTH, HEIGHT, PARAMS[]\n"; + fout_camera_ << "1 PINHOLE " << width_ << " " << height_ << " " + << std::fixed << std::setprecision(6) // 控制浮点数精度为10位 + << fx_ << " " << fy_ << " " << cx_ << " " << cy_ << std::endl; + fout_camera_.close(); } - grid_num.resize(length); - map_index.resize(length); - map_dist.resize(length); - update_flag.resize(length); - scan_value.resize(length); - - patch_size_total = patch_size * patch_size; - patch_size_half = static_cast(patch_size / 2); - patch_buffer.resize(patch_size_total); - warp_len = patch_size_total * patch_pyrimid_level; - border = (patch_size_half + 1) * (1 << patch_pyrimid_level); - - retrieve_voxel_points.reserve(length); - append_voxel_points.reserve(length); - - sub_feat_map.clear(); + grid_num_.resize(length_); + map_index_.resize(length_); + map_dist_.resize(length_); + update_flag_.resize(length_); + scan_value_.resize(length_); + + patch_size_total_ = patch_size_ * patch_size_; + patch_size_half_ = static_cast(patch_size_ / 2); + patch_buffer_.resize(patch_size_total_); + warp_len_ = patch_size_total_ * patch_pyrimid_level_; + border_ = (patch_size_half_ + 1) * (1 << patch_pyrimid_level_); + + retrieve_voxel_points_.reserve(length_); + append_voxel_points_.reserve(length_); + + sub_feat_map_.clear(); } -void VIOManager::resetGrid() -{ - fill(grid_num.begin(), grid_num.end(), TYPE_UNKNOWN); - fill(map_index.begin(), map_index.end(), 0); - fill(map_dist.begin(), map_dist.end(), 10000.0f); - fill(update_flag.begin(), update_flag.end(), 0); - fill(scan_value.begin(), scan_value.end(), 0.0f); +void VIOManager::ResetGrid() { + fill(grid_num_.begin(), grid_num_.end(), TYPE_UNKNOWN); + fill(map_index_.begin(), map_index_.end(), 0); + fill(map_dist_.begin(), map_dist_.end(), 10000.0f); + fill(update_flag_.begin(), update_flag_.end(), 0); + fill(scan_value_.begin(), scan_value_.end(), 0.0f); - retrieve_voxel_points.clear(); - retrieve_voxel_points.resize(length); + retrieve_voxel_points_.clear(); + retrieve_voxel_points_.resize(length_); - append_voxel_points.clear(); - append_voxel_points.resize(length); + append_voxel_points_.clear(); + append_voxel_points_.resize(length_); - total_points = 0; + total_points_ = 0; } // void VIOManager::resetRvizDisplay() // { - // sub_map_ray.clear(); - // sub_map_ray_fov.clear(); - // visual_sub_map_cur.clear(); - // visual_converged_point.clear(); - // map_cur_frame.clear(); - // sample_points.clear(); +// sub_map_ray.clear(); +// sub_map_ray_fov.clear(); +// visual_sub_map_cur.clear(); +// visual_converged_point.clear(); +// map_cur_frame.clear(); +// sample_points.clear(); // } -void VIOManager::computeProjectionJacobian(V3D p, MD(2, 3) & J) -{ +void VIOManager::ComputeProjectionJacobian(V3D p, MD(2, 3) & J) { + // 视觉SLAM十四讲公式(8.18) const double x = p[0]; const double y = p[1]; const double z_inv = 1. / p[2]; const double z_inv_2 = z_inv * z_inv; - J(0, 0) = fx * z_inv; + J(0, 0) = fx_ * z_inv; J(0, 1) = 0.0; - J(0, 2) = -fx * x * z_inv_2; + J(0, 2) = -fx_ * x * z_inv_2; J(1, 0) = 0.0; - J(1, 1) = fy * z_inv; - J(1, 2) = -fy * y * z_inv_2; + J(1, 1) = fy_ * z_inv; + J(1, 2) = -fy_ * y * z_inv_2; } -void VIOManager::getImagePatch(cv::Mat img, V2D pc, float *patch_tmp, int level) -{ +void VIOManager::GetImagePatch(cv::Mat img, V2D pc, float *patch_tmp, + int level) { const float u_ref = pc[0]; const float v_ref = pc[1]; const int scale = (1 << level); @@ -213,53 +215,96 @@ void VIOManager::getImagePatch(cv::Mat img, V2D pc, float *patch_tmp, int level) const float w_ref_tr = subpix_u_ref * (1.0 - subpix_v_ref); const float w_ref_bl = (1.0 - subpix_u_ref) * subpix_v_ref; const float w_ref_br = subpix_u_ref * subpix_v_ref; - for (int x = 0; x < patch_size; x++) - { - uint8_t *img_ptr = (uint8_t *)img.data + (v_ref_i - patch_size_half * scale + x * scale) * width + (u_ref_i - patch_size_half * scale); - for (int y = 0; y < patch_size; y++, img_ptr += scale) - { - patch_tmp[patch_size_total * level + x * patch_size + y] = - w_ref_tl * img_ptr[0] + w_ref_tr * img_ptr[scale] + w_ref_bl * img_ptr[scale * width] + w_ref_br * img_ptr[scale * width + scale]; + for (int x = 0; x < patch_size_; x++) { + uint8_t *img_ptr = + (uint8_t *)img.data + + (v_ref_i - patch_size_half_ * scale + x * scale) * width_ + + (u_ref_i - patch_size_half_ * scale); + for (int y = 0; y < patch_size_; y++, img_ptr += scale) { + patch_tmp[patch_size_total_ * level + x * patch_size_ + y] = + w_ref_tl * img_ptr[0] + w_ref_tr * img_ptr[scale] + + w_ref_bl * img_ptr[scale * width_] + + w_ref_br * img_ptr[scale * width_ + scale]; } } } -void VIOManager::insertPointIntoVoxelMap(VisualPoint *pt_new) -{ +#if 0 +void VIOManager::InsertPointIntoFeatureMap(VisualPoint *pt_new) { V3D pt_w(pt_new->pos_[0], pt_new->pos_[1], pt_new->pos_[2]); double voxel_size = 0.5; float loc_xyz[3]; - for (int j = 0; j < 3; j++) - { + for (int j = 0; j < 3; j++) { loc_xyz[j] = pt_w[j] / voxel_size; - if (loc_xyz[j] < 0) { loc_xyz[j] -= 1.0; } + if (loc_xyz[j] < 0) { + loc_xyz[j] -= 1.0; + } } - VOXEL_LOCATION position((int64_t)loc_xyz[0], (int64_t)loc_xyz[1], (int64_t)loc_xyz[2]); - auto iter = feat_map.find(position); - if (iter != feat_map.end()) - { + VOXEL_LOCATION position((int64_t)loc_xyz[0], (int64_t)loc_xyz[1], + (int64_t)loc_xyz[2]); + auto iter = feat_map_.find(position); + if (iter != feat_map_.end()) { iter->second->voxel_points.push_back(pt_new); iter->second->count++; + } else { + VOXEL_POINTS *ot = new VOXEL_POINTS(0); + ot->voxel_points.push_back(pt_new); + feat_map_[position] = ot; + } +} +#endif + +void VIOManager::InsertPointIntoFeatureMapLRU(VisualPoint *pt_new) { + V3D pt_w(pt_new->pos_[0], pt_new->pos_[1], pt_new->pos_[2]); + double voxel_size = 0.5; + float loc_xyz[3]; + for (int j = 0; j < 3; j++) { + loc_xyz[j] = pt_w[j] / voxel_size; + if (loc_xyz[j] < 0) { + loc_xyz[j] -= 1.0; + } } - else - { + VOXEL_LOCATION position((int64_t)loc_xyz[0], (int64_t)loc_xyz[1], + (int64_t)loc_xyz[2]); + auto iter = vp_map_.find(position); + if (iter != vp_map_.end()) { + iter->second->second->voxel_points.push_back(pt_new); + iter->second->second->count++; + // 更新的放至最前 + vp_data_.splice(vp_data_.begin(), vp_data_, iter->second); + iter->second = vp_data_.begin(); + } else { VOXEL_POINTS *ot = new VOXEL_POINTS(0); ot->voxel_points.push_back(pt_new); - feat_map[position] = ot; + vp_data_.push_front({position, {ot}}); + vp_map_.insert({position, vp_data_.begin()}); + + // LRU + if (vp_data_.size() >= lru_size_) { + // 删除一个尾部的数据 + vp_map_.erase(vp_data_.back().first); + delete vp_data_.back().second; + vp_data_.pop_back(); + } } } -void VIOManager::getWarpMatrixAffineHomography(const vk::AbstractCamera &cam, const V2D &px_ref, const V3D &xyz_ref, const V3D &normal_ref, - const SE3 &T_cur_ref, const int level_ref, Matrix2d &A_cur_ref) -{ - // create homography matrix +void VIOManager::GetWarpMatrixAffineHomography( + const vk::AbstractCamera &cam, const V2D &px_ref, const V3D &xyz_ref, + const V3D &normal_ref, const SE3 &T_cur_ref, const int level_ref, + Eigen::Matrix2d &A_cur_ref) { + // 构建单应矩阵 const V3D t = T_cur_ref.inverse().translation(); const Eigen::Matrix3d H_cur_ref = - T_cur_ref.rotation_matrix() * (normal_ref.dot(xyz_ref) * Eigen::Matrix3d::Identity() - t * normal_ref.transpose()); - // Compute affine warp matrix A_ref_cur using homography projection + T_cur_ref.rotationMatrix() * + (normal_ref.dot(xyz_ref) * Eigen::Matrix3d::Identity() - + t * normal_ref.transpose()); + // 单应投影计算仿射变换矩阵A_ref_cur const int kHalfPatchSize = 4; - V3D f_du_ref(cam.cam2world(px_ref + Eigen::Vector2d(kHalfPatchSize, 0) * (1 << level_ref))); - V3D f_dv_ref(cam.cam2world(px_ref + Eigen::Vector2d(0, kHalfPatchSize) * (1 << level_ref))); + V3D f_du_ref(cam.cam2world(px_ref + Eigen::Vector2d(kHalfPatchSize, 0) * + (1 << level_ref))); + V3D f_dv_ref(cam.cam2world(px_ref + Eigen::Vector2d(0, kHalfPatchSize) * + (1 << level_ref))); // f_du_ref = f_du_ref/f_du_ref[2]; // f_dv_ref = f_dv_ref/f_dv_ref[2]; const V3D f_cur(H_cur_ref * xyz_ref); @@ -272,66 +317,73 @@ void VIOManager::getWarpMatrixAffineHomography(const vk::AbstractCamera &cam, co A_cur_ref.col(1) = (px_dv_cur - px_cur) / kHalfPatchSize; } -void VIOManager::getWarpMatrixAffine(const vk::AbstractCamera &cam, const Vector2d &px_ref, const Vector3d &f_ref, const double depth_ref, - const SE3 &T_cur_ref, const int level_ref, const int pyramid_level, const int halfpatch_size, - Matrix2d &A_cur_ref) -{ +void VIOManager::GetWarpMatrixAffine( + const vk::AbstractCamera &cam, const Eigen::Vector2d &px_ref, + const Eigen::Vector3d &f_ref, const double depth_ref, const SE3 &T_cur_ref, + const int level_ref, const int pyramid_level, const int halfpatch_size, + Eigen::Matrix2d &A_cur_ref) { // Compute affine warp matrix A_ref_cur - const Vector3d xyz_ref(f_ref * depth_ref); - Vector3d xyz_du_ref(cam.cam2world(px_ref + Vector2d(halfpatch_size, 0) * (1 << level_ref) * (1 << pyramid_level))); - Vector3d xyz_dv_ref(cam.cam2world(px_ref + Vector2d(0, halfpatch_size) * (1 << level_ref) * (1 << pyramid_level))); + const Eigen::Vector3d xyz_ref(f_ref * depth_ref); + Eigen::Vector3d xyz_du_ref( + cam.cam2world(px_ref + Eigen::Vector2d(halfpatch_size, 0) * + (1 << level_ref) * (1 << pyramid_level))); + Eigen::Vector3d xyz_dv_ref( + cam.cam2world(px_ref + Eigen::Vector2d(0, halfpatch_size) * + (1 << level_ref) * (1 << pyramid_level))); xyz_du_ref *= xyz_ref[2] / xyz_du_ref[2]; xyz_dv_ref *= xyz_ref[2] / xyz_dv_ref[2]; - const Vector2d px_cur(cam.world2cam(T_cur_ref * (xyz_ref))); - const Vector2d px_du(cam.world2cam(T_cur_ref * (xyz_du_ref))); - const Vector2d px_dv(cam.world2cam(T_cur_ref * (xyz_dv_ref))); + const Eigen::Vector2d px_cur(cam.world2cam(T_cur_ref * (xyz_ref))); + const Eigen::Vector2d px_du(cam.world2cam(T_cur_ref * (xyz_du_ref))); + const Eigen::Vector2d px_dv(cam.world2cam(T_cur_ref * (xyz_dv_ref))); A_cur_ref.col(0) = (px_du - px_cur) / halfpatch_size; A_cur_ref.col(1) = (px_dv - px_cur) / halfpatch_size; } -void VIOManager::warpAffine(const Matrix2d &A_cur_ref, const cv::Mat &img_ref, const Vector2d &px_ref, const int level_ref, const int search_level, - const int pyramid_level, const int halfpatch_size, float *patch) -{ +void VIOManager::WarpAffine(const Eigen::Matrix2d &A_cur_ref, + const cv::Mat &img_ref, + const Eigen::Vector2d &px_ref, const int level_ref, + const int search_level, const int pyramid_level, + const int halfpatch_size, float *patch) { const int patch_size = halfpatch_size * 2; - const Matrix2f A_ref_cur = A_cur_ref.inverse().cast(); - if (isnan(A_ref_cur(0, 0))) - { - printf("Affine warp is NaN, probably camera has no translation\n"); // TODO + // 参考帧到当前帧的变换 + const Eigen::Matrix2f A_ref_cur = A_cur_ref.inverse().cast(); + if (isnan(A_ref_cur(0, 0))) { + printf("Affine warp is NaN, probably camera has no translation\n"); // TODO return; } float *patch_ptr = patch; - for (int y = 0; y < patch_size; ++y) - { - for (int x = 0; x < patch_size; ++x) //, ++patch_ptr) + for (int y = 0; y < patch_size; ++y) { + for (int x = 0; x < patch_size; ++x) //, ++patch_ptr) { - Vector2f px_patch(x - halfpatch_size, y - halfpatch_size); + Eigen::Vector2f px_patch(x - halfpatch_size, y - halfpatch_size); px_patch *= (1 << search_level); px_patch *= (1 << pyramid_level); - const Vector2f px(A_ref_cur * px_patch + px_ref.cast()); - if (px[0] < 0 || px[1] < 0 || px[0] >= img_ref.cols - 1 || px[1] >= img_ref.rows - 1) - patch_ptr[patch_size_total * pyramid_level + y * patch_size + x] = 0; + const Eigen::Vector2f px(A_ref_cur * px_patch + px_ref.cast()); + if (px[0] < 0 || px[1] < 0 || px[0] >= img_ref.cols - 1 || + px[1] >= img_ref.rows - 1) + patch_ptr[patch_size_total_ * pyramid_level + y * patch_size + x] = 0; else - patch_ptr[patch_size_total * pyramid_level + y * patch_size + x] = (float)vk::interpolateMat_8u(img_ref, px[0], px[1]); + patch_ptr[patch_size_total_ * pyramid_level + y * patch_size + x] = + (float)vk::interpolateMat_8u(img_ref, px[0], px[1]); } } } -int VIOManager::getBestSearchLevel(const Matrix2d &A_cur_ref, const int max_level) -{ +int VIOManager::GetBestSearchLevel(const Eigen::Matrix2d &A_cur_ref, + const int max_level) { // Compute patch level in other image int search_level = 0; double D = A_cur_ref.determinant(); - while (D > 3.0 && search_level < max_level) - { + while (D > 3.0 && search_level < max_level) { search_level += 1; D *= 0.25; } return search_level; } -double VIOManager::calculateNCC(float *ref_patch, float *cur_patch, int patch_size) -{ +double VIOManager::CalculateNCC(float *ref_patch, float *cur_patch, + int patch_size) { double sum_ref = std::accumulate(ref_patch, ref_patch + patch_size, 0.0); double mean_ref = sum_ref / patch_size; @@ -339,8 +391,7 @@ double VIOManager::calculateNCC(float *ref_patch, float *cur_patch, int patch_si double mean_curr = sum_cur / patch_size; double numerator = 0, demoniator1 = 0, demoniator2 = 0; - for (int i = 0; i < patch_size; i++) - { + for (int i = 0; i < patch_size; i++) { double n = (ref_patch[i] - mean_ref) * (cur_patch[i] - mean_curr); numerator += n; demoniator1 += (ref_patch[i] - mean_ref) * (ref_patch[i] - mean_ref); @@ -349,146 +400,486 @@ double VIOManager::calculateNCC(float *ref_patch, float *cur_patch, int patch_si return numerator / sqrt(demoniator1 * demoniator2 + 1e-10); } -void VIOManager::retrieveFromVisualSparseMap(cv::Mat img, vector &pg, const unordered_map &plane_map) -{ - if (feat_map.size() <= 0) return; +#if 0 +void VIOManager::RetrieveFromVisualSparseMap( + cv::Mat img, std::vector &pv_list, + const std::unordered_map &voxel_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(); + visual_submap_->reset(); - // Controls whether to include the visual submap from the previous frame. - sub_feat_map.clear(); + // 控制是否添加来自前一帧的视觉子地图 + sub_feat_map_.clear(); float voxel_size = 0.5; - if (!normal_en) warp_map.clear(); + // 法向量默认开启 + if (!normal_en_) warp_map_.clear(); - cv::Mat depth_img = cv::Mat::zeros(height, width, CV_32FC1); + cv::Mat depth_img = cv::Mat::zeros(height_, width_, CV_32FC1); + // data连续内存 float *it = (float *)depth_img.data; - // float it[height * width] = {0.0}; + int loc_xyz[3]; - // double t_insert, t_depth, t_position; - // t_insert=t_depth=t_position=0; + // 遍历点云数据，生成深度图 + for (int i = 0; i < pv_list.size(); i++) { + V3D pt_w = pv_list[i].point_w; - int loc_xyz[3]; + 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]); - // printf("A0. initial depthmap: %.6lf \n", omp_get_wtime() - ts0); - // double ts1 = 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; + } - // printf("pg size: %zu \n", pg.size()); + V3D pt_c(new_frame_->w2f(pt_w)); - for (int i = 0; i < pg.size(); i++) - { - // double t0 = omp_get_wtime(); + if (pt_c[2] > 0) { + V2D px; + px = new_frame_->cam_->world2cam(pt_c); + // 记录深度 + if (new_frame_->cam_->isInFrame(px.cast(), border_)) { + float depth = pt_c[2]; + int col = int(px[0]); + int row = int(px[1]); + it[width_ * row + col] = depth; + } + } + } - V3D pt_w = pg[i].point_w; + // 筛选和更新视觉子地图 + std::vector DeleteKeyList; // sub_feat_map_中无用的数据 - 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; } + for (auto &iter : sub_feat_map_) { + VOXEL_LOCATION position = iter.first; + + auto corre_voxel = feat_map_.find(position); + + 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_.rotationMatrix() * pt->normal_); + V3D dir(new_frame_->T_f_w_ * pt->pos_); + if (dir[2] < 0) continue; + + V2D pc(new_frame_->w2c(pt->pos_)); + if (new_frame_->cam_->isInFrame(pc.cast(), border_)) { + 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; + Eigen::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); + } } - VOXEL_LOCATION position(loc_xyz[0], loc_xyz[1], loc_xyz[2]); + } + + // 光线追踪模型，用来补充特征，默认关闭 + 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; - // t_position += omp_get_wtime()-t0; - // double t1 = omp_get_wtime(); + // sub_map_ray.push_back(pt); // cloud_visual_sub_map + // add_sample = true; + + V3D norm_vec(new_frame_->T_f_w_.rotationMatrix() * 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_)); - auto iter = sub_feat_map.find(position); - if (iter == sub_feat_map.end()) { sub_feat_map[position] = 0; } - else { iter->second = 0; } + 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); - // t_insert += omp_get_wtime()-t1; - // double t2 = omp_get_wtime(); + 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; + Eigen::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 = voxel_map.find(sample_pos); + if (iter != voxel_map.end()) { + VoxelOctoTree *current_octo; + current_octo = iter->second->FindCorrespond(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); + } + + for (int i = 0; i < length_; i++) { + if (grid_num_[i] == TYPE_MAP) { + VisualPoint *pt = retrieve_voxel_points_[i]; + // visual_sub_map_cur.push_back(pt); // before + + V2D pc(new_frame_->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_discontinous = 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_discontinous = true; + break; + } + } + if (depth_discontinous) break; + } + if (depth_discontinous) continue; + + Feature *ref_ftr; + std::vector patch_wrap(warp_len_); + + int search_level; + Eigen::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 photometric_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++) { + photometric_errors += (patch_temp[ind] - patch_cache[ind]) * + (patch_temp[ind] - patch_cache[ind]); + } + count++; + } + photometric_errors = photometric_errors / count; + if (photometric_errors < phtometric_errors_min) { + phtometric_errors_min = photometric_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_.rotationMatrix() * pt->normal_).normalized(); + + V3D pf(ref_ftr->T_f_w_ * pt->pos_); + + 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; + } + } + + 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()); + } + // 获取当前帧特征点对应的patch + 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_); + } + } + total_points_ = visual_submap_->voxel_points.size(); + + printf("[ VIO ] Retrieve %d points from visual sparse map\n", + total_points_); +} +#endif + +void VIOManager::RetrieveFromVisualSparseMapLRU( + cv::Mat img, std::vector &pv_list, + const std::unordered_map::iterator> &vm_map) { + if (vp_map_.size() <= 0) return; + double ts0 = omp_get_wtime(); + + // resetRvizDisplay(); + visual_submap_->reset(); + + // 控制是否添加来自前一帧的视觉子地图 + 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); + // data连续内存 + float *it = (float *)depth_img.data; + + int loc_xyz[3]; + + // 遍历点云数据，生成深度图 + for (int i = 0; i < pv_list.size(); i++) { + V3D pt_w = pv_list[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]); + + // 添加当前帧点云对应的体素 + auto iter = sub_feat_map_.find(position); + if (iter == sub_feat_map_.end()) { + sub_feat_map_[position] = 0; + } else { + iter->second = 0; + } V3D pt_c(new_frame_->w2f(pt_w)); - if (pt_c[2] > 0) - { + 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 (new_frame_->cam_->isInFrame(px.cast(), border_)) { float depth = pt_c[2]; int col = int(px[0]); int row = int(px[1]); - it[width * row + col] = depth; + 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); + // 筛选和更新视觉子地图 + std::vector DeleteKeyList; // sub_feat_map_中无用的数据 - // double t1 = omp_get_wtime(); - vector DeleteKeyList; - - for (auto &iter : sub_feat_map) - { + 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(); + auto corre_voxel = vp_map_.find(position); - if (corre_voxel != feat_map.end()) - { + if (corre_voxel != vp_map_.end()) { bool voxel_in_fov = false; - std::vector &voxel_points = corre_voxel->second->voxel_points; + std::vector &voxel_points = + corre_voxel->second->second->voxel_points; int voxel_num = voxel_points.size(); - for (int i = 0; i < voxel_num; i++) - { + 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 norm_vec(new_frame_->T_f_w_.rotationMatrix() * 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); + if (new_frame_->cam_->isInFrame(pc.cast(), border_)) { 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_); + int index = static_cast(pc[1] / grid_size_) * grid_n_width_ + + static_cast(pc[0] / grid_size_); + grid_num_[index] = TYPE_MAP; + Eigen::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 (cur_dist <= map_dist_[index]) { + map_dist_[index] = cur_dist; + retrieve_voxel_points_[index] = pt; } } } - if (!voxel_in_fov) { DeleteKeyList.push_back(position); } + 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; + // 光线追踪模型，用来补充特征，默认关闭 + 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) * @@ -500,33 +891,32 @@ void VIOManager::retrieveFromVisualSparseMap(cv::Mat img, vector & // vector sample_points_temp; // bool add_sample = false; - for (const auto &it : rays_with_sample_points[i]) - { + 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++) - { + 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; } + 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_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()) - { + auto corre_feat_map = vp_map_.find(sample_pos); + if (corre_feat_map != vp_map_.end()) { bool voxel_in_fov = false; - std::vector &voxel_points = corre_feat_map->second->voxel_points; + std::vector &voxel_points = + corre_feat_map->second->second->voxel_points; int voxel_num = voxel_points.size(); if (voxel_num == 0) continue; - for (int j = 0; j < voxel_num; j++) - { + for (int j = 0; j < voxel_num; j++) { VisualPoint *pt = voxel_points[j]; if (pt == nullptr) continue; @@ -535,52 +925,48 @@ void VIOManager::retrieveFromVisualSparseMap(cv::Mat img, vector & // 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 norm_vec(new_frame_->T_f_w_.rotationMatrix() * 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 + // 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); + 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_); + int index = static_cast(pc[1] / grid_size_) * grid_n_width_ + + static_cast(pc[0] / grid_size_); + grid_num_[index] = TYPE_MAP; + Eigen::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 (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; + if (voxel_in_fov) sub_feat_map_[sample_pos] = 0; break; - } - else - { + } 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()) - { + auto iter = vm_map.find(sample_pos); + if (iter != vm_map.end()) { VoxelOctoTree *current_octo; - current_octo = iter->second->find_correspond(sample_point_w); - if (current_octo->plane_ptr_->is_plane_) - { + current_octo = iter->second->second->FindCorrespond(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); + visual_submap_->add_from_voxel_map.push_back(plane_center); break; } } @@ -590,342 +976,315 @@ void VIOManager::retrieveFromVisualSparseMap(cv::Mat img, vector & } } - for (auto &key : DeleteKeyList) - { - sub_feat_map.erase(key); + 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 + // 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++) - { + // 深度连续性检查 + bool depth_discontinous = 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])]; + 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; + if (delta_dist > 0.5) { + depth_discontinous = true; break; } } - if (depth_continous) break; + if (depth_discontinous) break; } - if (depth_continous) continue; - - // t_2 += omp_get_wtime() - t_1; + if (depth_discontinous) continue; - // t_1 = omp_get_wtime(); Feature *ref_ftr; - std::vector patch_wrap(warp_len); + std::vector patch_wrap(warp_len_); int search_level; - Matrix2d A_cur_ref_zero; + Eigen::Matrix2d A_cur_ref_zero; if (!pt->is_normal_initialized_) continue; - if (normal_en) - { + if (normal_en_) { float phtometric_errors_min = std::numeric_limits::max(); - if (pt->obs_.size() == 1) - { + 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) - { + } 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; + float photometric_errors = 0.0; int count = 0; - for (auto itm = pt->obs_.begin(), itme = pt->obs_.end(); itm != itme; ++itm) - { + 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]); + for (int ind = 0; ind < patch_size_total_; ind++) { + photometric_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; + photometric_errors = photometric_errors / count; + if (photometric_errors < phtometric_errors_min) { + phtometric_errors_min = photometric_errors; ref_ftr = ref_patch_temp; } } pt->ref_patch = ref_ftr; pt->has_ref_patch_ = true; + } else { + ref_ftr = pt->ref_patch; } - else { ref_ftr = pt->ref_patch; } - } - else - { + } 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(); - + // 计算仿射变换矩阵和搜索层级 + if (normal_en_) { + V3D norm_vec = + (ref_ftr->T_f_w_.rotationMatrix() * 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); + 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 = 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); + } 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); + search_level = GetBestSearchLevel(A_cur_ref_zero, 2); Warp *ot = new Warp(search_level, A_cur_ref_zero); - warp_map[ref_ftr->id_] = ot; + 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()); + 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); + // 获取当前帧特征点对应的patch + 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]); + 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) - { + // 默认关闭 + 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; + 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_); + + // LRU update + float update_xyz[3]; + for (int j = 0; j < 3; j++) { + update_xyz[j] = pt->pos_[j] / voxel_size; + if (update_xyz[j] < 0) { + update_xyz[j] -= 1.0; + } + } + VOXEL_LOCATION update_position((int64_t)update_xyz[0], + (int64_t)update_xyz[1], + (int64_t)update_xyz[2]); + auto update_iter = vp_map_.find(update_position); + if (update_iter != vp_map_.end()) { + // move to the front of LRU list + vp_data_.splice(vp_data_.begin(), vp_data_, update_iter->second); + update_iter->second = vp_data_.begin(); + } } } - total_points = visual_submap->voxel_points.size(); + total_points_ = visual_submap_->voxel_points.size(); - // double t3 = omp_get_wtime(); - // cout<<"C. addSubSparseMap: "<= 0; level--) - { - if (inverse_composition_en) - { - has_ref_patch_cache = false; - updateStateInverse(img, level); + compute_jacobian_time_ = update_ekf_time_ = 0.0; + + for (int level = patch_pyrimid_level_ - 1; level >= 0; level--) { + if (inverse_composition_en_) { + has_ref_patch_cache_ = false; + UpdateStateInverse(img, level); + } else { + // 实际执行 + UpdateState(img, level); } - else - updateState(img, level); } - state->cov -= G * state->cov; - updateFrameState(*state); + state_->cov -= G_ * state_->cov; + UpdateFrameState(*state_); } -void VIOManager::generateVisualMapPoints(cv::Mat img, vector &pg) -{ - if (pg.size() <= 10) return; +void VIOManager::GenerateVisualMapPoints(cv::Mat img, + std::vector &pv_list) { + if (pv_list.size() <= 10) return; - // double t0 = omp_get_wtime(); - for (int i = 0; i < pg.size(); i++) - { - if (pg[i].normal == V3D(0, 0, 0)) continue; + // 遍历当前帧点云数据，添加特征点至待处理列表中 + for (int i = 0; i < pv_list.size(); i++) { + if (pv_list[i].normal == V3D(0, 0, 0)) continue; - V3D pt = pg[i].point_w; + V3D pt = pv_list[i].point_w; V2D pc(new_frame_->w2c(pt)); + // 20px is the patch size in the matcher + if (new_frame_->cam_->isInFrame(pc.cast(), border_)) { + int index = static_cast(pc[1] / grid_size_) * grid_n_width_ + + static_cast(pc[0] / grid_size_); - if (new_frame_->cam_->isInFrame(pc.cast(), border)) // 20px is the patch size in the matcher - { - int index = static_cast(pc[1] / grid_size) * grid_n_width + static_cast(pc[0] / grid_size); - - if (grid_num[index] != TYPE_MAP) - { + if (grid_num_[index] != TYPE_MAP) { + // Shi-Tomasi 角点检测 float cur_value = vk::shiTomasiScore(img, pc[0], pc[1]); // if (cur_value < 5) continue; - if (cur_value > scan_value[index]) - { - scan_value[index] = cur_value; - append_voxel_points[index] = pg[i]; - grid_num[index] = TYPE_POINTCLOUD; + if (cur_value > scan_value_[index]) { + scan_value_[index] = cur_value; + append_voxel_points_[index] = pv_list[i]; + grid_num_[index] = TYPE_POINTCLOUD; } } } } - for (int j = 0; j < visual_submap->add_from_voxel_map.size(); j++) - { - V3D pt = visual_submap->add_from_voxel_map[j].point_w; + // 遍历视觉局部地图（可能包含历史点等），添加特征点至待处理列表中 + for (int j = 0; j < visual_submap_->add_from_voxel_map.size(); j++) { + V3D pt = visual_submap_->add_from_voxel_map[j].point_w; V2D pc(new_frame_->w2c(pt)); + // 20px is the patch size in the matcher + if (new_frame_->cam_->isInFrame(pc.cast(), border_)) { + int index = static_cast(pc[1] / grid_size_) * grid_n_width_ + + static_cast(pc[0] / grid_size_); - if (new_frame_->cam_->isInFrame(pc.cast(), border)) // 20px is the patch size in the matcher - { - int index = static_cast(pc[1] / grid_size) * grid_n_width + static_cast(pc[0] / grid_size); - - if (grid_num[index] != TYPE_MAP) - { + if (grid_num_[index] != TYPE_MAP) { float cur_value = vk::shiTomasiScore(img, pc[0], pc[1]); - if (cur_value > scan_value[index]) - { - scan_value[index] = cur_value; - append_voxel_points[index] = visual_submap->add_from_voxel_map[j]; - grid_num[index] = TYPE_POINTCLOUD; + if (cur_value > scan_value_[index]) { + scan_value_[index] = cur_value; + append_voxel_points_[index] = visual_submap_->add_from_voxel_map[j]; + grid_num_[index] = TYPE_POINTCLOUD; } } } } - // double t_b1 = omp_get_wtime() - t0; - // t0 = omp_get_wtime(); - int add = 0; - for (int i = 0; i < length; i++) - { - if (grid_num[i] == TYPE_POINTCLOUD) // && (scan_value[i]>=50)) + for (int i = 0; i < length_; i++) { + if (grid_num_[i] == TYPE_POINTCLOUD) // && (scan_value[i]>=50)) { - pointWithVar pt_var = append_voxel_points[i]; + pointWithVar pt_var = append_voxel_points_[i]; V3D pt = pt_var.point_w; - V3D norm_vec(new_frame_->T_f_w_.rotation_matrix() * pt_var.normal); + V3D norm_vec(new_frame_->T_f_w_.rotationMatrix() * pt_var.normal); V3D dir(new_frame_->T_f_w_ * pt); dir.normalize(); double cos_theta = dir.dot(norm_vec); // if(std::fabs(cos_theta)<0.34) continue; // 70 degree V2D pc(new_frame_->w2c(pt)); - float *patch = new float[patch_size_total]; - getImagePatch(img, pc, patch, 0); + float *patch = new float[patch_size_total_]; + GetImagePatch(img, pc, patch, 0); VisualPoint *pt_new = new VisualPoint(pt); - Vector3d f = cam->cam2world(pc); - Feature *ftr_new = new Feature(pt_new, patch, pc, f, new_frame_->T_f_w_, 0); - ftr_new->img_ = img; - ftr_new->id_ = new_frame_->id_; - ftr_new->inv_expo_time_ = state->inv_expo_time; + Eigen::Vector3d f = cam_->cam2world(pc); + Feature *feature_new = + new Feature(pt_new, patch, pc, f, new_frame_->T_f_w_, 0); + feature_new->img_ = img; + feature_new->id_ = new_frame_->id_; + feature_new->inv_expo_time_ = state_->inv_expo_time; - pt_new->addFrameRef(ftr_new); + pt_new->addFrameRef(feature_new); pt_new->covariance_ = pt_var.var; pt_new->is_normal_initialized_ = true; - if (cos_theta < 0) { pt_new->normal_ = -pt_var.normal; } - else { pt_new->normal_ = pt_var.normal; } - + if (cos_theta < 0) { + pt_new->normal_ = -pt_var.normal; + } else { + pt_new->normal_ = pt_var.normal; + } + pt_new->previous_normal_ = pt_new->normal_; - insertPointIntoVoxelMap(pt_new); + InsertPointIntoFeatureMapLRU(pt_new); add += 1; // map_cur_frame.push_back(pt_new); } } - // double t_b2 = omp_get_wtime() - t0; - printf("[ VIO ] Append %d new visual map points\n", add); - // printf("pg.size: %d \n", pg.size()); - // printf("B1. : %.6lf \n", t_b1); - // printf("B2. : %.6lf \n", t_b2); } -void VIOManager::updateVisualMapPoints(cv::Mat img) -{ - if (total_points == 0) return; +void VIOManager::UpdateVisualMapPoints(cv::Mat img) { + if (total_points_ == 0) return; int update_num = 0; SE3 pose_cur = new_frame_->T_f_w_; - for (int i = 0; i < total_points; i++) - { - VisualPoint *pt = visual_submap->voxel_points[i]; + for (int i = 0; i < total_points_; i++) { + VisualPoint *pt = visual_submap_->voxel_points[i]; if (pt == nullptr) continue; - if (pt->is_converged_) - { + if (pt->is_converged_) { pt->deleteNonRefPatchFeatures(); continue; } V2D pc(new_frame_->w2c(pt->pos_)); bool add_flag = false; - - float *patch_temp = new float[patch_size_total]; - getImagePatch(img, pc, patch_temp, 0); + + float *patch_temp = new float[patch_size_total_]; + GetImagePatch(img, pc, patch_temp, 0); // TODO: condition: distance and view_angle // Step 1: time Feature *last_feature = pt->obs_.back(); @@ -935,96 +1294,102 @@ void VIOManager::updateVisualMapPoints(cv::Mat img) SE3 pose_ref = last_feature->T_f_w_; SE3 delta_pose = pose_ref * pose_cur.inverse(); double delta_p = delta_pose.translation().norm(); - double delta_theta = (delta_pose.rotation_matrix().trace() > 3.0 - 1e-6) ? 0.0 : std::acos(0.5 * (delta_pose.rotation_matrix().trace() - 1)); - if (delta_p > 0.5 || delta_theta > 0.3) add_flag = true; // 0.5 || 0.3 + double delta_theta = + (delta_pose.rotationMatrix().trace() > 3.0 - 1e-6) + ? 0.0 + : std::acos(0.5 * (delta_pose.rotationMatrix().trace() - 1)); + if (delta_p > 0.5 || delta_theta > 0.3) add_flag = true; // 0.5 || 0.3 // Step 3: pixel distance - Vector2d last_px = last_feature->px_; + Eigen::Vector2d last_px = last_feature->px_; double pixel_dist = (pc - last_px).norm(); if (pixel_dist > 40) add_flag = true; // Maintain the size of 3D point observation features. - if (pt->obs_.size() >= 30) - { + if (pt->obs_.size() >= 30) { Feature *ref_ftr; pt->findMinScoreFeature(new_frame_->pos(), ref_ftr); pt->deleteFeatureRef(ref_ftr); // cout<<"pt->obs_.size() exceed 20 !!!!!!"<cam2world(pc); - Feature *ftr_new = new Feature(pt, patch_temp, pc, f, new_frame_->T_f_w_, visual_submap->search_levels[i]); + update_flag_[i] = 1; + Eigen::Vector3d f = cam_->cam2world(pc); + Feature *ftr_new = new Feature(pt, patch_temp, pc, f, new_frame_->T_f_w_, + visual_submap_->search_levels[i]); ftr_new->img_ = img; ftr_new->id_ = new_frame_->id_; - ftr_new->inv_expo_time_ = state->inv_expo_time; + ftr_new->inv_expo_time_ = state_->inv_expo_time; pt->addFrameRef(ftr_new); } } printf("[ VIO ] Update %d points in visual submap\n", update_num); } -void VIOManager::updateReferencePatch(const unordered_map &plane_map) -{ - if (total_points == 0) return; +void VIOManager::UpdateReferencePatch( + const std::unordered_map &plane_map) { + if (total_points_ == 0) return; - for (int i = 0; i < visual_submap->voxel_points.size(); i++) - { - VisualPoint *pt = visual_submap->voxel_points[i]; + for (int i = 0; i < visual_submap_->voxel_points.size(); i++) { + VisualPoint *pt = visual_submap_->voxel_points[i]; if (!pt->is_normal_initialized_) continue; if (pt->is_converged_) continue; if (pt->obs_.size() <= 5) continue; - if (update_flag[i] == 0) continue; + if (update_flag_[i] == 0) continue; const V3D &p_w = pt->pos_; float loc_xyz[3]; - for (int j = 0; j < 3; j++) - { + for (int j = 0; j < 3; j++) { loc_xyz[j] = p_w[j] / 0.5; - if (loc_xyz[j] < 0) { loc_xyz[j] -= 1.0; } + if (loc_xyz[j] < 0) { + loc_xyz[j] -= 1.0; + } } - VOXEL_LOCATION position((int64_t)loc_xyz[0], (int64_t)loc_xyz[1], (int64_t)loc_xyz[2]); + VOXEL_LOCATION position((int64_t)loc_xyz[0], (int64_t)loc_xyz[1], + (int64_t)loc_xyz[2]); auto iter = plane_map.find(position); - if (iter != plane_map.end()) - { + if (iter != plane_map.end()) { VoxelOctoTree *current_octo; - current_octo = iter->second->find_correspond(p_w); - if (current_octo->plane_ptr_->is_plane_) - { + current_octo = iter->second->FindCorrespond(p_w); + if (current_octo->plane_ptr_->is_plane_) { VoxelPlane &plane = *current_octo->plane_ptr_; - float dis_to_plane = plane.normal_(0) * p_w(0) + plane.normal_(1) * p_w(1) + plane.normal_(2) * p_w(2) + plane.d_; + float dis_to_plane = plane.normal_(0) * p_w(0) + + plane.normal_(1) * p_w(1) + + plane.normal_(2) * p_w(2) + plane.d_; float dis_to_plane_abs = fabs(dis_to_plane); - float dis_to_center = (plane.center_(0) - p_w(0)) * (plane.center_(0) - p_w(0)) + - (plane.center_(1) - p_w(1)) * (plane.center_(1) - p_w(1)) + (plane.center_(2) - p_w(2)) * (plane.center_(2) - p_w(2)); + float dis_to_center = + (plane.center_(0) - p_w(0)) * (plane.center_(0) - p_w(0)) + + (plane.center_(1) - p_w(1)) * (plane.center_(1) - p_w(1)) + + (plane.center_(2) - p_w(2)) * (plane.center_(2) - p_w(2)); float range_dis = sqrt(dis_to_center - dis_to_plane * dis_to_plane); - if (range_dis <= 3 * plane.radius_) - { + if (range_dis <= 3 * plane.radius_) { Eigen::Matrix J_nq; J_nq.block<1, 3>(0, 0) = p_w - plane.center_; J_nq.block<1, 3>(0, 3) = -plane.normal_; double sigma_l = J_nq * plane.plane_var_ * J_nq.transpose(); - sigma_l += plane.normal_.transpose() * pt->covariance_ * plane.normal_; - - if (dis_to_plane_abs < 3 * sqrt(sigma_l)) - { - // V3D norm_vec(new_frame_->T_f_w_.rotation_matrix() * plane.normal_); - // V3D pf(new_frame_->T_f_w_ * pt->pos_); - // V3D pf_ref(pt->ref_patch->T_f_w_ * pt->pos_); - // V3D norm_vec_ref(pt->ref_patch->T_f_w_.rotation_matrix() * + sigma_l += + plane.normal_.transpose() * pt->covariance_ * plane.normal_; + // 检验更新法线方向 + if (dis_to_plane_abs < 3 * sqrt(sigma_l)) { + // V3D norm_vec(new_frame_->T_f_w_.rotationMatrix() * + // plane.normal_); V3D pf(new_frame_->T_f_w_ * pt->pos_); V3D + // pf_ref(pt->ref_patch->T_f_w_ * pt->pos_); V3D + // norm_vec_ref(pt->ref_patch->T_f_w_.rotationMatrix() * // plane.normal); double cos_ref = pf_ref.dot(norm_vec_ref); - - if (pt->previous_normal_.dot(plane.normal_) < 0) { pt->normal_ = -plane.normal_; } - else { pt->normal_ = plane.normal_; } + + if (pt->previous_normal_.dot(plane.normal_) < 0) { + pt->normal_ = -plane.normal_; + } else { + pt->normal_ = plane.normal_; + } double normal_update = (pt->normal_ - pt->previous_normal_).norm(); pt->previous_normal_ = pt->normal_; - if (normal_update < 0.0001 && pt->obs_.size() > 10) - { + if (normal_update < 0.0001 && pt->obs_.size() > 10) { pt->is_converged_ = true; // visual_converged_point.push_back(pt); } @@ -1033,9 +1398,9 @@ void VIOManager::updateReferencePatch(const unordered_mapobs_.begin(), ite = pt->obs_.end(); it != ite; ++it) - { + 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 NCC_up = 0.0; @@ -1046,37 +1411,39 @@ void VIOManager::updateReferencePatch(const unordered_mapT_f_w_ * pt->pos_; - V3D norm_vec = ref_patch_temp->T_f_w_.rotation_matrix() * pt->normal_; + V3D norm_vec = ref_patch_temp->T_f_w_.rotationMatrix() * pt->normal_; pf.normalize(); double cos_angle = pf.dot(norm_vec); // if(fabs(cos_angle) < 0.86) continue; // 20 degree float ref_mean; - if (abs(ref_patch_temp->mean_) < 1e-6) - { - float ref_sum = std::accumulate(patch_temp, patch_temp + patch_size_total, 0.0); - ref_mean = ref_sum / patch_size_total; + if (abs(ref_patch_temp->mean_) < 1e-6) { + float ref_sum = + std::accumulate(patch_temp, patch_temp + patch_size_total_, 0.0); + ref_mean = ref_sum / patch_size_total_; ref_patch_temp->mean_ = ref_mean; } - for (auto itm = pt->obs_.begin(), itme = pt->obs_.end(); itm != itme; ++itm) - { + 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_; float other_mean; - if (abs((*itm)->mean_) < 1e-6) - { - float other_sum = std::accumulate(patch_cache, patch_cache + patch_size_total, 0.0); - other_mean = other_sum / patch_size_total; + if (abs((*itm)->mean_) < 1e-6) { + float other_sum = std::accumulate( + patch_cache, patch_cache + patch_size_total_, 0.0); + other_mean = other_sum / patch_size_total_; (*itm)->mean_ = other_mean; } - for (int ind = 0; ind < patch_size_total; ind++) - { - NCC_up += (patch_temp[ind] - ref_mean) * (patch_cache[ind] - other_mean); - NCC_down1 += (patch_temp[ind] - ref_mean) * (patch_temp[ind] - ref_mean); - NCC_down2 += (patch_cache[ind] - other_mean) * (patch_cache[ind] - other_mean); + for (int ind = 0; ind < patch_size_total_; ind++) { + NCC_up += + (patch_temp[ind] - ref_mean) * (patch_cache[ind] - other_mean); + NCC_down1 += + (patch_temp[ind] - ref_mean) * (patch_temp[ind] - ref_mean); + NCC_down2 += + (patch_cache[ind] - other_mean) * (patch_cache[ind] - other_mean); } NCC += fabs(NCC_up / sqrt(NCC_down1 * NCC_down2)); count++; @@ -1088,28 +1455,163 @@ void VIOManager::updateReferencePatch(const unordered_mapscore_ = score; - if (score > score_max) - { + if (score > score_max) { score_max = score; pt->ref_patch = ref_patch_temp; pt->has_ref_patch_ = true; } } + } +} + +void VIOManager::UpdateReferencePatch( + const std::unordered_map::iterator> &vm_map) { + if (total_points_ == 0) return; + + for (int i = 0; i < visual_submap_->voxel_points.size(); i++) { + VisualPoint *pt = visual_submap_->voxel_points[i]; + + if (!pt->is_normal_initialized_) continue; + if (pt->is_converged_) continue; + if (pt->obs_.size() <= 5) continue; + if (update_flag_[i] == 0) continue; + + const V3D &p_w = pt->pos_; + float loc_xyz[3]; + for (int j = 0; j < 3; j++) { + loc_xyz[j] = p_w[j] / 0.5; + if (loc_xyz[j] < 0) { + loc_xyz[j] -= 1.0; + } + } + VOXEL_LOCATION position((int64_t)loc_xyz[0], (int64_t)loc_xyz[1], + (int64_t)loc_xyz[2]); + auto iter = vm_map.find(position); + if (iter != vm_map.end()) { + VoxelOctoTree *current_octo; + current_octo = iter->second->second->FindCorrespond(p_w); + if (current_octo->plane_ptr_->is_plane_) { + VoxelPlane &plane = *current_octo->plane_ptr_; + float dis_to_plane = plane.normal_(0) * p_w(0) + + plane.normal_(1) * p_w(1) + + plane.normal_(2) * p_w(2) + plane.d_; + float dis_to_plane_abs = fabs(dis_to_plane); + float dis_to_center = + (plane.center_(0) - p_w(0)) * (plane.center_(0) - p_w(0)) + + (plane.center_(1) - p_w(1)) * (plane.center_(1) - p_w(1)) + + (plane.center_(2) - p_w(2)) * (plane.center_(2) - p_w(2)); + float range_dis = sqrt(dis_to_center - dis_to_plane * dis_to_plane); + if (range_dis <= 3 * plane.radius_) { + Eigen::Matrix J_nq; + J_nq.block<1, 3>(0, 0) = p_w - plane.center_; + J_nq.block<1, 3>(0, 3) = -plane.normal_; + double sigma_l = J_nq * plane.plane_var_ * J_nq.transpose(); + sigma_l += + plane.normal_.transpose() * pt->covariance_ * plane.normal_; + // 检验更新法线方向 + if (dis_to_plane_abs < 3 * sqrt(sigma_l)) { + // V3D norm_vec(new_frame_->T_f_w_.rotationMatrix() * + // plane.normal_); V3D pf(new_frame_->T_f_w_ * pt->pos_); V3D + // pf_ref(pt->ref_patch->T_f_w_ * pt->pos_); V3D + // norm_vec_ref(pt->ref_patch->T_f_w_.rotationMatrix() * + // plane.normal); double cos_ref = pf_ref.dot(norm_vec_ref); + + if (pt->previous_normal_.dot(plane.normal_) < 0) { + pt->normal_ = -plane.normal_; + } else { + pt->normal_ = plane.normal_; + } + + double normal_update = (pt->normal_ - pt->previous_normal_).norm(); + + pt->previous_normal_ = pt->normal_; + + if (normal_update < 0.0001 && pt->obs_.size() > 10) { + pt->is_converged_ = true; + // visual_converged_point.push_back(pt); + } + } + } + } + } + + // 计算NCC得分(公式12)选择最优参考图像块 + float score_max = -1000.; + 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 NCC_up = 0.0; + float NCC_down1 = 0.0; + float NCC_down2 = 0.0; + float NCC = 0.0; + float score = 0.0; + int count = 0; + V3D pf = ref_patch_temp->T_f_w_ * pt->pos_; + V3D norm_vec = ref_patch_temp->T_f_w_.rotationMatrix() * pt->normal_; + pf.normalize(); + double cos_angle = pf.dot(norm_vec); + // if(fabs(cos_angle) < 0.86) continue; // 20 degree + + float ref_mean; + if (abs(ref_patch_temp->mean_) < 1e-6) { + float ref_sum = + std::accumulate(patch_temp, patch_temp + patch_size_total_, 0.0); + ref_mean = ref_sum / patch_size_total_; + ref_patch_temp->mean_ = ref_mean; + } + + 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_; + + float other_mean; + if (abs((*itm)->mean_) < 1e-6) { + float other_sum = std::accumulate( + patch_cache, patch_cache + patch_size_total_, 0.0); + other_mean = other_sum / patch_size_total_; + (*itm)->mean_ = other_mean; + } + + for (int ind = 0; ind < patch_size_total_; ind++) { + NCC_up += + (patch_temp[ind] - ref_mean) * (patch_cache[ind] - other_mean); + NCC_down1 += + (patch_temp[ind] - ref_mean) * (patch_temp[ind] - ref_mean); + NCC_down2 += + (patch_cache[ind] - other_mean) * (patch_cache[ind] - other_mean); + } + NCC += fabs(NCC_up / sqrt(NCC_down1 * NCC_down2)); + count++; + } + + NCC = NCC / count; + + score = NCC + cos_angle; + + ref_patch_temp->score_ = score; + + if (score > score_max) { + score_max = score; + pt->ref_patch = ref_patch_temp; + pt->has_ref_patch_ = true; + } + } } } -void VIOManager::projectPatchFromRefToCur(const unordered_map &plane_map) -{ - if (total_points == 0) return; +void VIOManager::ProjectPatchFromRefToCur() { + if (total_points_ == 0) return; // if(new_frame_->id_ != 2) return; //124 int patch_size = 25; - string dir = string(ROOT_DIR) + "Log/ref_cur_combine/"; + std::string dir = std::string(ROOT_DIR) + "Log/ref_cur_combine/"; - cv::Mat result = cv::Mat::zeros(height, width, CV_8UC1); - cv::Mat result_normal = cv::Mat::zeros(height, width, CV_8UC1); - cv::Mat result_dense = cv::Mat::zeros(height, width, CV_8UC1); + cv::Mat result = cv::Mat::zeros(height_, width_, CV_8UC1); + cv::Mat result_normal = cv::Mat::zeros(height_, width_, CV_8UC1); + cv::Mat result_dense = cv::Mat::zeros(height_, width_, CV_8UC1); cv::Mat img_photometric_error = new_frame_->img_.clone(); @@ -1117,26 +1619,23 @@ void VIOManager::projectPatchFromRefToCur(const unordered_mapvoxel_points.size(); i++) - { - VisualPoint *pt = visual_submap->voxel_points[i]; + for (int i = 0; i < visual_submap_->voxel_points.size(); i++) { + VisualPoint *pt = visual_submap_->voxel_points[i]; - if (pt->is_normal_initialized_) - { + if (pt->is_normal_initialized_) { Feature *ref_ftr; ref_ftr = pt->ref_patch; // Feature* ref_ftr; V2D pc(new_frame_->w2c(pt->pos_)); V2D pc_prior(new_frame_->w2c_prior(pt->pos_)); - V3D norm_vec(ref_ftr->T_f_w_.rotation_matrix() * pt->normal_); + V3D norm_vec(ref_ftr->T_f_w_.rotationMatrix() * pt->normal_); V3D pf(ref_ftr->T_f_w_ * pt->pos_); if (pf.dot(norm_vec) < 0) norm_vec = -norm_vec; @@ -1146,11 +1645,12 @@ void VIOManager::projectPatchFromRefToCur(const unordered_mapimg_; SE3 T_cur_ref = new_frame_->T_f_w_ * ref_ftr->T_f_w_.inverse(); - Matrix2d A_cur_ref; - getWarpMatrixAffineHomography(*cam, ref_ftr->px_, pf, norm_vec, T_cur_ref, 0, A_cur_ref); + Eigen::Matrix2d A_cur_ref; + GetWarpMatrixAffineHomography(*cam_, ref_ftr->px_, pf, norm_vec, + T_cur_ref, 0, A_cur_ref); // const Matrix2f A_ref_cur = A_cur_ref.inverse().cast(); - int search_level = getBestSearchLevel(A_cur_ref.inverse(), 2); + int search_level = GetBestSearchLevel(A_cur_ref.inverse(), 2); double D = A_cur_ref.determinant(); if (D > 3) continue; @@ -1162,59 +1662,85 @@ void VIOManager::projectPatchFromRefToCur(const unordered_mapinv_expo_time_ * visual_submap->warp_patch[i][ind] - state->inv_expo_time * patch_buffer[ind]) * - (ref_ftr->inv_expo_time_ * visual_submap->warp_patch[i][ind] - state->inv_expo_time * patch_buffer[ind]); + for (int ind = 0; ind < patch_size_total_; ind++) { + error_est += + (ref_ftr->inv_expo_time_ * visual_submap_->warp_patch[i][ind] - + state_->inv_expo_time * patch_buffer_[ind]) * + (ref_ftr->inv_expo_time_ * visual_submap_->warp_patch[i][ind] - + state_->inv_expo_time * patch_buffer_[ind]); } - std::string ref_est = "ref_est " + std::to_string(1.0 / ref_ftr->inv_expo_time_); - std::string cur_est = "cur_est " + std::to_string(1.0 / state->inv_expo_time); + std::string ref_est = + "ref_est " + std::to_string(1.0 / ref_ftr->inv_expo_time_); + std::string cur_est = + "cur_est " + std::to_string(1.0 / state_->inv_expo_time); std::string cur_propa = "cur_gt " + std::to_string(error_gt); std::string cur_optimize = "cur_est " + std::to_string(error_est); - cv::putText(ref_cur_combine_temp, ref_est, cv::Point2f(ref_ftr->px_[0] + img_cur.cols - 40, ref_ftr->px_[1] + 40), cv::FONT_HERSHEY_COMPLEX, 0.4, - cv::Scalar(0, 255, 0), 1, 8, 0); - - cv::putText(ref_cur_combine_temp, cur_est, cv::Point2f(pc[0] - 40, pc[1] + 40), cv::FONT_HERSHEY_COMPLEX, 0.4, cv::Scalar(0, 255, 0), 1, 8, 0); - cv::putText(ref_cur_combine_temp, cur_propa, cv::Point2f(pc[0] - 40, pc[1] + 60), cv::FONT_HERSHEY_COMPLEX, 0.4, cv::Scalar(0, 0, 255), 1, 8, - 0); - cv::putText(ref_cur_combine_temp, cur_optimize, cv::Point2f(pc[0] - 40, pc[1] + 80), cv::FONT_HERSHEY_COMPLEX, 0.4, cv::Scalar(0, 255, 0), 1, 8, + cv::putText(ref_cur_combine_temp, ref_est, + cv::Point2f(ref_ftr->px_[0] + img_cur.cols - 40, + ref_ftr->px_[1] + 40), + cv::FONT_HERSHEY_COMPLEX, 0.4, cv::Scalar(0, 255, 0), 1, 8, 0); - cv::rectangle(ref_cur_combine_temp, cv::Point2f(ref_ftr->px_[0] + img_cur.cols - radius, ref_ftr->px_[1] - radius), - cv::Point2f(ref_ftr->px_[0] + img_cur.cols + radius, ref_ftr->px_[1] + radius), cv::Scalar(0, 0, 255), 1); - cv::rectangle(ref_cur_combine_temp, cv::Point2f(pc[0] - radius, pc[1] - radius), cv::Point2f(pc[0] + radius, pc[1] + radius), + cv::putText(ref_cur_combine_temp, cur_est, + cv::Point2f(pc[0] - 40, pc[1] + 40), cv::FONT_HERSHEY_COMPLEX, + 0.4, cv::Scalar(0, 255, 0), 1, 8, 0); + cv::putText(ref_cur_combine_temp, cur_propa, + cv::Point2f(pc[0] - 40, pc[1] + 60), cv::FONT_HERSHEY_COMPLEX, + 0.4, cv::Scalar(0, 0, 255), 1, 8, 0); + cv::putText(ref_cur_combine_temp, cur_optimize, + cv::Point2f(pc[0] - 40, pc[1] + 80), cv::FONT_HERSHEY_COMPLEX, + 0.4, cv::Scalar(0, 255, 0), 1, 8, 0); + + cv::rectangle(ref_cur_combine_temp, + cv::Point2f(ref_ftr->px_[0] + img_cur.cols - radius, + ref_ftr->px_[1] - radius), + cv::Point2f(ref_ftr->px_[0] + img_cur.cols + radius, + ref_ftr->px_[1] + radius), + cv::Scalar(0, 0, 255), 1); + cv::rectangle(ref_cur_combine_temp, + cv::Point2f(pc[0] - radius, pc[1] - radius), + cv::Point2f(pc[0] + radius, pc[1] + radius), cv::Scalar(0, 255, 0), 1); - cv::rectangle(ref_cur_combine_temp, cv::Point2f(pc_prior[0] - radius, pc_prior[1] - radius), - cv::Point2f(pc_prior[0] + radius, pc_prior[1] + radius), cv::Scalar(255, 255, 255), 1); - cv::circle(ref_cur_combine_temp, cv::Point2f(ref_ftr->px_[0] + img_cur.cols, ref_ftr->px_[1]), 1, cv::Scalar(0, 0, 255), -1, 8); - cv::circle(ref_cur_combine_temp, cv::Point2f(pc[0], pc[1]), 1, cv::Scalar(0, 255, 0), -1, 8); - cv::circle(ref_cur_combine_temp, cv::Point2f(pc_prior[0], pc_prior[1]), 1, cv::Scalar(255, 255, 255), -1, 8); - cv::imwrite(dir + std::to_string(new_frame_->id_) + "_" + std::to_string(ref_ftr->id_) + "_" + std::to_string(num) + ".png", + cv::rectangle(ref_cur_combine_temp, + cv::Point2f(pc_prior[0] - radius, pc_prior[1] - radius), + cv::Point2f(pc_prior[0] + radius, pc_prior[1] + radius), + cv::Scalar(255, 255, 255), 1); + cv::circle(ref_cur_combine_temp, + cv::Point2f(ref_ftr->px_[0] + img_cur.cols, ref_ftr->px_[1]), + 1, cv::Scalar(0, 0, 255), -1, 8); + cv::circle(ref_cur_combine_temp, cv::Point2f(pc[0], pc[1]), 1, + cv::Scalar(0, 255, 0), -1, 8); + cv::circle(ref_cur_combine_temp, cv::Point2f(pc_prior[0], pc_prior[1]), 1, + cv::Scalar(255, 255, 255), -1, 8); + cv::imwrite(dir + std::to_string(new_frame_->id_) + "_" + + std::to_string(ref_ftr->id_) + "_" + std::to_string(num) + + ".png", ref_cur_combine_temp); std::vector> pixel_warp_matrix; - for (int y = 0; y < patch_size; ++y) - { - vector pixel_warp_vec; - for (int x = 0; x < patch_size; ++x) //, ++patch_ptr) + for (int y = 0; y < patch_size; ++y) { + std::vector pixel_warp_vec; + for (int x = 0; x < patch_size; ++x) //, ++patch_ptr) { - Vector2f px_patch(x - patch_size / 2, y - patch_size / 2); + Eigen::Vector2f px_patch(x - patch_size / 2, y - patch_size / 2); px_patch *= (1 << search_level); - const Vector2f px_ref(px_patch + ref_ftr->px_.cast()); - uint8_t pixel_value = (uint8_t)vk::interpolateMat_8u(img_ref, px_ref[0], px_ref[1]); - - const Vector2f px(A_cur_ref.cast() * px_patch + pc.cast()); - if (px[0] < 0 || px[1] < 0 || px[0] >= img_cur.cols - 1 || px[1] >= img_cur.rows - 1) + const Eigen::Vector2f px_ref(px_patch + ref_ftr->px_.cast()); + uint8_t pixel_value = + (uint8_t)vk::interpolateMat_8u(img_ref, px_ref[0], px_ref[1]); + + const Eigen::Vector2f px(A_cur_ref.cast() * px_patch + + pc.cast()); + if (px[0] < 0 || px[1] < 0 || px[0] >= img_cur.cols - 1 || + px[1] >= img_cur.rows - 1) continue; - else - { + else { pixel_member pixel_warp; pixel_warp.pixel_pos << px[0], px[1]; pixel_warp.pixel_value = pixel_value; @@ -1229,11 +1755,9 @@ void VIOManager::projectPatchFromRefToCur(const unordered_map pixel_warp_row = pixel_warp_matrix[i]; - for (int j = 0; j < pixel_warp_row.size(); j++) - { + for (int i = 0; i < pixel_warp_matrix.size(); i++) { + std::vector pixel_warp_row = pixel_warp_matrix[i]; + for (int j = 0; j < pixel_warp_row.size(); j++) { float x_temp = pixel_warp_row[j].pixel_pos[0]; float y_temp = pixel_warp_row[j].pixel_pos[1]; if (x_temp < x_min) x_min = x_temp; @@ -1246,27 +1770,27 @@ void VIOManager::projectPatchFromRefToCur(const unordered_map(); - for (int i = x_min_i; i < x_max_i; i++) - { - for (int j = y_min_i; j < y_max_i; j++) - { + Eigen::Matrix2f A_cur_ref_Inv = A_cur_ref.inverse().cast(); + for (int i = x_min_i; i < x_max_i; i++) { + for (int j = y_min_i; j < y_max_i; j++) { Eigen::Vector2f pc_temp(i, j); - Vector2f px_patch = A_cur_ref_Inv * (pc_temp - pc.cast()); - if (px_patch[0] > (-patch_size / 2 * (1 << search_level)) && px_patch[0] < (patch_size / 2 * (1 << search_level)) && - px_patch[1] > (-patch_size / 2 * (1 << search_level)) && px_patch[1] < (patch_size / 2 * (1 << search_level))) - { - const Vector2f px_ref(px_patch + ref_ftr->px_.cast()); - uint8_t pixel_value = (uint8_t)vk::interpolateMat_8u(img_ref, px_ref[0], px_ref[1]); - it_normal[width * j + i] = pixel_value; + Eigen::Vector2f px_patch = + A_cur_ref_Inv * (pc_temp - pc.cast()); + if (px_patch[0] > (-patch_size / 2 * (1 << search_level)) && + px_patch[0] < (patch_size / 2 * (1 << search_level)) && + px_patch[1] > (-patch_size / 2 * (1 << search_level)) && + px_patch[1] < (patch_size / 2 * (1 << search_level))) { + const Eigen::Vector2f px_ref(px_patch + ref_ftr->px_.cast()); + uint8_t pixel_value = + (uint8_t)vk::interpolateMat_8u(img_ref, px_ref[0], px_ref[1]); + it_normal[width_ * j + i] = pixel_value; } } } } } - for (int i = 0; i < visual_submap->voxel_points.size(); i++) - { - VisualPoint *pt = visual_submap->voxel_points[i]; + for (int i = 0; i < visual_submap_->voxel_points.size(); i++) { + VisualPoint *pt = visual_submap_->voxel_points[i]; if (!pt->is_normal_initialized_) continue; @@ -1274,32 +1798,35 @@ void VIOManager::projectPatchFromRefToCur(const unordered_mapw2c(pt->pos_)); ref_ftr = pt->ref_patch; - Matrix2d A_cur_ref; - 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(), 0, 0, - patch_size_half, A_cur_ref); - int search_level = getBestSearchLevel(A_cur_ref.inverse(), 2); + Eigen::Matrix2d A_cur_ref; + 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(), 0, 0, + patch_size_half_, A_cur_ref); + int search_level = GetBestSearchLevel(A_cur_ref.inverse(), 2); double D = A_cur_ref.determinant(); if (D > 3) continue; cv::Mat img_cur = new_frame_->img_; cv::Mat img_ref = ref_ftr->img_; - for (int y = 0; y < patch_size; ++y) - { - for (int x = 0; x < patch_size; ++x) //, ++patch_ptr) + for (int y = 0; y < patch_size; ++y) { + for (int x = 0; x < patch_size; ++x) //, ++patch_ptr) { - Vector2f px_patch(x - patch_size / 2, y - patch_size / 2); + Eigen::Vector2f px_patch(x - patch_size / 2, y - patch_size / 2); px_patch *= (1 << search_level); - const Vector2f px_ref(px_patch + ref_ftr->px_.cast()); - uint8_t pixel_value = (uint8_t)vk::interpolateMat_8u(img_ref, px_ref[0], px_ref[1]); - - const Vector2f px(A_cur_ref.cast() * px_patch + pc.cast()); - if (px[0] < 0 || px[1] < 0 || px[0] >= img_cur.cols - 1 || px[1] >= img_cur.rows - 1) + const Eigen::Vector2f px_ref(px_patch + ref_ftr->px_.cast()); + uint8_t pixel_value = + (uint8_t)vk::interpolateMat_8u(img_ref, px_ref[0], px_ref[1]); + + const Eigen::Vector2f px(A_cur_ref.cast() * px_patch + + pc.cast()); + if (px[0] < 0 || px[1] < 0 || px[0] >= img_cur.cols - 1 || + px[1] >= img_cur.rows - 1) continue; - else - { + else { int col = int(px[0]); int row = int(px[1]); - it[width * row + col] = pixel_value; + it[width_ * row + col] = pixel_value; } } } @@ -1316,33 +1843,33 @@ void VIOManager::projectPatchFromRefToCur(const unordered_mapimg_, ref_cur_combine_error); - cv::imwrite(dir + std::to_string(new_frame_->id_) + "_0_" + ".png", ref_cur_combine); + cv::imwrite(dir + std::to_string(new_frame_->id_) + "_0_" + ".png", + ref_cur_combine); cv::imwrite(dir + std::to_string(new_frame_->id_) + +"_0_" + "photometric" ".png", ref_cur_combine_error); - cv::imwrite(dir + std::to_string(new_frame_->id_) + "_0_" + "normal" + ".png", ref_cur_combine_normal); + cv::imwrite(dir + std::to_string(new_frame_->id_) + "_0_" + "normal" + ".png", + ref_cur_combine_normal); } -void VIOManager::precomputeReferencePatches(int level) -{ +void VIOManager::PrecomputeReferencePatches(int level) { double t1 = omp_get_wtime(); - if (total_points == 0) return; + if (total_points_ == 0) return; MD(1, 2) Jimg; MD(2, 3) Jdpi; MD(1, 3) Jdphi, Jdp, JdR, Jdt; - const int H_DIM = total_points * patch_size_total; + const int H_DIM = total_points_ * patch_size_total_; - H_sub_inv.resize(H_DIM, 6); - H_sub_inv.setZero(); + H_sub_inv_.resize(H_DIM, 6); + H_sub_inv_.setZero(); M3D p_w_hat; - for (int i = 0; i < total_points; i++) - { + for (int i = 0; i < total_points_; i++) { const int scale = (1 << level); - VisualPoint *pt = visual_submap->voxel_points[i]; + VisualPoint *pt = visual_submap_->voxel_points[i]; cv::Mat img = pt->ref_patch->img_; if (pt == nullptr) continue; @@ -1350,9 +1877,9 @@ void VIOManager::precomputeReferencePatches(int level) double depth((pt->pos_ - pt->ref_patch->pos()).norm()); V3D pf = pt->ref_patch->f_ * depth; V2D pc = pt->ref_patch->px_; - M3D R_ref_w = pt->ref_patch->T_f_w_.rotation_matrix(); + M3D R_ref_w = pt->ref_patch->T_f_w_.rotationMatrix(); - computeProjectionJacobian(pf, Jdpi); + ComputeProjectionJacobian(pf, Jdpi); p_w_hat << SKEW_SYM_MATRX(pt->pos_); const float u_ref = pc[0]; @@ -1366,21 +1893,26 @@ void VIOManager::precomputeReferencePatches(int level) const float w_ref_bl = (1.0 - subpix_u_ref) * subpix_v_ref; const float w_ref_br = subpix_u_ref * subpix_v_ref; - for (int x = 0; x < patch_size; x++) - { - uint8_t *img_ptr = (uint8_t *)img.data + (v_ref_i + x * scale - patch_size_half * scale) * width + u_ref_i - patch_size_half * scale; - for (int y = 0; y < patch_size; ++y, img_ptr += scale) - { + for (int x = 0; x < patch_size_; x++) { + uint8_t *img_ptr = + (uint8_t *)img.data + + (v_ref_i + x * scale - patch_size_half_ * scale) * width_ + u_ref_i - + patch_size_half_ * scale; + for (int y = 0; y < patch_size_; ++y, img_ptr += scale) { float du = - 0.5f * - ((w_ref_tl * img_ptr[scale] + w_ref_tr * img_ptr[scale * 2] + w_ref_bl * img_ptr[scale * width + scale] + - w_ref_br * img_ptr[scale * width + scale * 2]) - - (w_ref_tl * img_ptr[-scale] + w_ref_tr * img_ptr[0] + w_ref_bl * img_ptr[scale * width - scale] + w_ref_br * img_ptr[scale * width])); - float dv = - 0.5f * - ((w_ref_tl * img_ptr[scale * width] + w_ref_tr * img_ptr[scale + scale * width] + w_ref_bl * img_ptr[width * scale * 2] + - w_ref_br * img_ptr[width * scale * 2 + scale]) - - (w_ref_tl * img_ptr[-scale * width] + w_ref_tr * img_ptr[-scale * width + scale] + w_ref_bl * img_ptr[0] + w_ref_br * img_ptr[scale])); + 0.5f * ((w_ref_tl * img_ptr[scale] + w_ref_tr * img_ptr[scale * 2] + + w_ref_bl * img_ptr[scale * width_ + scale] + + w_ref_br * img_ptr[scale * width_ + scale * 2]) - + (w_ref_tl * img_ptr[-scale] + w_ref_tr * img_ptr[0] + + w_ref_bl * img_ptr[scale * width_ - scale] + + w_ref_br * img_ptr[scale * width_])); + float dv = 0.5f * ((w_ref_tl * img_ptr[scale * width_] + + w_ref_tr * img_ptr[scale + scale * width_] + + w_ref_bl * img_ptr[width_ * scale * 2] + + w_ref_br * img_ptr[width_ * scale * 2 + scale]) - + (w_ref_tl * img_ptr[-scale * width_] + + w_ref_tr * img_ptr[-scale * width_ + scale] + + w_ref_bl * img_ptr[0] + w_ref_br * img_ptr[scale])); Jimg << du, dv; Jimg = Jimg * (1.0 / scale); @@ -1388,29 +1920,30 @@ void VIOManager::precomputeReferencePatches(int level) JdR = Jimg * Jdpi * R_ref_w * p_w_hat; Jdt = -Jimg * Jdpi * R_ref_w; - H_sub_inv.block<1, 6>(i * patch_size_total + x * patch_size + y, 0) << JdR, Jdt; + H_sub_inv_.block<1, 6>(i * patch_size_total_ + x * patch_size_ + y, 0) + << JdR, + Jdt; } } } - has_ref_patch_cache = true; + has_ref_patch_cache_ = true; } -void VIOManager::updateStateInverse(cv::Mat img, int level) -{ - if (total_points == 0) return; - StatesGroup old_state = (*state); +void VIOManager::UpdateStateInverse(cv::Mat img, int level) { + if (total_points_ == 0) return; + StatesGroup old_state = (*state_); V2D pc; MD(1, 2) Jimg; MD(2, 3) Jdpi; MD(1, 3) Jdphi, Jdp, JdR, Jdt; - VectorXd z; - MatrixXd H_sub; + Eigen::VectorXd z; + Eigen::MatrixXd H_sub; bool EKF_end = false; float last_error = std::numeric_limits::max(); - compute_jacobian_time = update_ekf_time = 0.0; + compute_jacobian_time_ = update_ekf_time_ = 0.0; M3D P_wi_hat; bool z_init = true; - const int H_DIM = total_points * patch_size_total; + const int H_DIM = total_points_ * patch_size_total_; z.resize(H_DIM); z.setZero(); @@ -1418,33 +1951,31 @@ void VIOManager::updateStateInverse(cv::Mat img, int level) H_sub.resize(H_DIM, 6); H_sub.setZero(); - for (int iteration = 0; iteration < max_iterations; iteration++) - { + for (int iteration = 0; iteration < max_iterations_; iteration++) { double t1 = omp_get_wtime(); double count_outlier = 0; - if (has_ref_patch_cache == false) precomputeReferencePatches(level); + if (has_ref_patch_cache_ == false) PrecomputeReferencePatches(level); int n_meas = 0; float error = 0.0; - M3D Rwi(state->rot_end); - V3D Pwi(state->pos_end); + M3D Rwi(state_->rot_end); + V3D Pwi(state_->pos_end); P_wi_hat << SKEW_SYM_MATRX(Pwi); - Rcw = Rci * Rwi.transpose(); - Pcw = -Rci * Rwi.transpose() * Pwi + Pci; + Rcw_ = Rci_ * Rwi.transpose(); + Pcw_ = -Rci_ * Rwi.transpose() * Pwi + Pci_; M3D p_hat; - for (int i = 0; i < total_points; i++) - { + for (int i = 0; i < total_points_; i++) { float patch_error = 0.0; const int scale = (1 << level); - VisualPoint *pt = visual_submap->voxel_points[i]; + VisualPoint *pt = visual_submap_->voxel_points[i]; if (pt == nullptr) continue; - V3D pf = Rcw * pt->pos_ + Pcw; - pc = cam->world2cam(pf); + V3D pf = Rcw_ * pt->pos_ + Pcw_; + pc = cam_->world2cam(pf); const float u_ref = pc[0]; const float v_ref = pc[1]; @@ -1457,290 +1988,295 @@ void VIOManager::updateStateInverse(cv::Mat img, int level) const float w_ref_bl = (1.0 - subpix_u_ref) * subpix_v_ref; const float w_ref_br = subpix_u_ref * subpix_v_ref; - vector P = visual_submap->warp_patch[i]; - for (int x = 0; x < patch_size; x++) - { - uint8_t *img_ptr = (uint8_t *)img.data + (v_ref_i + x * scale - patch_size_half * scale) * width + u_ref_i - patch_size_half * scale; - for (int y = 0; y < patch_size; ++y, img_ptr += scale) - { - double res = w_ref_tl * img_ptr[0] + w_ref_tr * img_ptr[scale] + w_ref_bl * img_ptr[scale * width] + - w_ref_br * img_ptr[scale * width + scale] - P[patch_size_total * level + x * patch_size + y]; - z(i * patch_size_total + x * patch_size + y) = res; + std::vector P = visual_submap_->warp_patch[i]; + for (int x = 0; x < patch_size_; x++) { + uint8_t *img_ptr = + (uint8_t *)img.data + + (v_ref_i + x * scale - patch_size_half_ * scale) * width_ + + u_ref_i - patch_size_half_ * scale; + for (int y = 0; y < patch_size_; ++y, img_ptr += scale) { + double res = w_ref_tl * img_ptr[0] + w_ref_tr * img_ptr[scale] + + w_ref_bl * img_ptr[scale * width_] + + w_ref_br * img_ptr[scale * width_ + scale] - + P[patch_size_total_ * level + x * patch_size_ + y]; + z(i * patch_size_total_ + x * patch_size_ + y) = res; patch_error += res * res; - MD(1, 3) J_dR = H_sub_inv.block<1, 3>(i * patch_size_total + x * patch_size + y, 0); - MD(1, 3) J_dt = H_sub_inv.block<1, 3>(i * patch_size_total + x * patch_size + y, 3); + MD(1, 3) + J_dR = H_sub_inv_.block<1, 3>( + i * patch_size_total_ + x * patch_size_ + y, 0); + MD(1, 3) + J_dt = H_sub_inv_.block<1, 3>( + i * patch_size_total_ + x * patch_size_ + y, 3); JdR = J_dR * Rwi + J_dt * P_wi_hat * Rwi; Jdt = J_dt * Rwi; - H_sub.block<1, 6>(i * patch_size_total + x * patch_size + y, 0) << JdR, Jdt; + H_sub.block<1, 6>(i * patch_size_total_ + x * patch_size_ + y, 0) + << JdR, + Jdt; n_meas++; } } - visual_submap->errors[i] = patch_error; + visual_submap_->errors[i] = patch_error; error += patch_error; } error = error / n_meas; - compute_jacobian_time += omp_get_wtime() - t1; + compute_jacobian_time_ += omp_get_wtime() - t1; double t3 = omp_get_wtime(); - if (error <= last_error) - { - old_state = (*state); + if (error <= last_error) { + old_state = (*state_); last_error = error; auto &&H_sub_T = H_sub.transpose(); - H_T_H.setZero(); - G.setZero(); - H_T_H.block<6, 6>(0, 0) = H_sub_T * H_sub; - MD(DIM_STATE, DIM_STATE) &&K_1 = (H_T_H + (state->cov / img_point_cov).inverse()).inverse(); + H_T_H_.setZero(); + G_.setZero(); + H_T_H_.block<6, 6>(0, 0) = H_sub_T * H_sub; + Mat19d &&K_1 = + (H_T_H_ + (state_->cov / img_point_cov_).inverse()).inverse(); auto &&HTz = H_sub_T * z; - auto vec = (*state_propagat) - (*state); - G.block(0, 0) = K_1.block(0, 0) * H_T_H.block<6, 6>(0, 0); - auto solution = -K_1.block(0, 0) * HTz + vec - G.block(0, 0) * vec.block<6, 1>(0, 0); - (*state) += solution; + auto vec = (*state_propagat_) - (*state_); + G_.block(0, 0) = + K_1.block(0, 0) * H_T_H_.block<6, 6>(0, 0); + auto solution = -K_1.block(0, 0) * HTz + vec - + G_.block(0, 0) * vec.block<6, 1>(0, 0); + (*state_) += solution; auto &&rot_add = solution.block<3, 1>(0, 0); auto &&t_add = solution.block<3, 1>(3, 0); - if ((rot_add.norm() * 57.3f < 0.001f) && (t_add.norm() * 100.0f < 0.001f)) { EKF_end = true; } - } - else - { - (*state) = old_state; + if ((rot_add.norm() * 57.3f < 0.001f) && + (t_add.norm() * 100.0f < 0.001f)) { + EKF_end = true; + } + } else { + (*state_) = old_state; EKF_end = true; } - update_ekf_time += omp_get_wtime() - t3; + update_ekf_time_ += omp_get_wtime() - t3; - if (iteration == max_iterations || EKF_end) break; + if (iteration == max_iterations_ || EKF_end) break; } } -void VIOManager::updateState(cv::Mat img, int level) -{ - if (total_points == 0) return; - StatesGroup old_state = (*state); +void VIOManager::UpdateState(cv::Mat img, int level) { + if (total_points_ == 0) return; + StatesGroup old_state = (*state_); - VectorXd z; - MatrixXd H_sub; + Eigen::VectorXd z; + Eigen::MatrixXd H_sub; bool EKF_end = false; float last_error = std::numeric_limits::max(); - const int H_DIM = total_points * patch_size_total; + const int H_DIM = total_points_ * patch_size_total_; z.resize(H_DIM); z.setZero(); - H_sub.resize(H_DIM, 7); + H_sub.resize(H_DIM, 7); // R, t, τ H_sub.setZero(); - for (int iteration = 0; iteration < max_iterations; iteration++) - { + for (int iteration = 0; iteration < max_iterations_; iteration++) { double t1 = omp_get_wtime(); - M3D Rwi(state->rot_end); - V3D Pwi(state->pos_end); - Rcw = Rci * Rwi.transpose(); - Pcw = -Rci * Rwi.transpose() * Pwi + Pci; - Jdp_dt = Rci * Rwi.transpose(); - + M3D Rwi(state_->rot_end); + V3D Pwi(state_->pos_end); + Rcw_ = Rci_ * Rwi.transpose(); + Pcw_ = -Rci_ * Rwi.transpose() * Pwi + Pci_; + Jdp_dt_ = Rci_ * Rwi.transpose(); + float error = 0.0; int n_meas = 0; // int max_threads = omp_get_max_threads(); // int desired_threads = std::min(max_threads, total_points); // omp_set_num_threads(desired_threads); - - #ifdef MP_EN - omp_set_num_threads(MP_PROC_NUM); - #pragma omp parallel for reduction(+:error, n_meas) - #endif - for (int i = 0; i < total_points; i++) - { - // printf("thread is %d, i=%d, i address is %p\n", omp_get_thread_num(), i, &i); - MD(1, 2) Jimg; - MD(2, 3) Jdpi; - MD(1, 3) Jdphi, Jdp, JdR, Jdt; + +#ifdef MP_EN + omp_set_num_threads(MP_PROC_NUM); +#pragma omp parallel for reduction(+ : error, n_meas) +#endif + for (int i = 0; i < total_points_; i++) { + // printf("thread is %d, i=%d, i address is %p\n", omp_get_thread_num(), + // i, &i); + MD(1, 2) Jimg; //图像雅可比 + MD(2, 3) Jdpi; //投影雅可比 + MD(1, 3) Jdphi, Jdp, JdR, Jdt; //状态雅可比 float patch_error = 0.0; - int search_level = visual_submap->search_levels[i]; + int search_level = visual_submap_->search_levels[i]; int pyramid_level = level + search_level; - int scale = (1 << pyramid_level); + int scale = (1 << pyramid_level); //等价于 scale = pow(2, pyramid_level) float inv_scale = 1.0f / scale; - VisualPoint *pt = visual_submap->voxel_points[i]; + VisualPoint *pt = visual_submap_->voxel_points[i]; if (pt == nullptr) continue; - V3D pf = Rcw * pt->pos_ + Pcw; - V2D pc = cam->world2cam(pf); - - computeProjectionJacobian(pf, Jdpi); + V3D pf = Rcw_ * pt->pos_ + Pcw_; + V2D pc = cam_->world2cam(pf); + // 检查投影点是否远离图像边界，确保有足够空间计算梯度 + int total_margin = patch_size_half_ * scale + scale; + if (pc[0] < total_margin || pc[0] > (img.cols - total_margin) || + pc[1] < total_margin || pc[1] > (img.rows - total_margin)) { + continue; + } + // 计算∂u/∂ẟξ + ComputeProjectionJacobian(pf, Jdpi); M3D p_hat; p_hat << SKEW_SYM_MATRX(pf); - + // 图像中的浮点坐标 float u_ref = pc[0]; float v_ref = pc[1]; + // 坐标整数部分 int u_ref_i = floorf(pc[0] / scale) * scale; int v_ref_i = floorf(pc[1] / scale) * scale; + // 坐标小数部分 float subpix_u_ref = (u_ref - u_ref_i) / scale; float subpix_v_ref = (v_ref - v_ref_i) / scale; + // 左上、右上、左下、右下的权重，用于插值计算 float w_ref_tl = (1.0 - subpix_u_ref) * (1.0 - subpix_v_ref); float w_ref_tr = subpix_u_ref * (1.0 - subpix_v_ref); float w_ref_bl = (1.0 - subpix_u_ref) * subpix_v_ref; float w_ref_br = subpix_u_ref * subpix_v_ref; - vector P = visual_submap->warp_patch[i]; - double inv_ref_expo = visual_submap->inv_expo_list[i]; - // ROS_ERROR("inv_ref_expo: %.3lf, state->inv_expo_time: %.3lf\n", inv_ref_expo, state->inv_expo_time); + // 参考帧的光度和曝光时间 + std::vector P = visual_submap_->warp_patch[i]; + double inv_ref_expo = visual_submap_->inv_expo_list[i]; + + for (int x = 0; x < patch_size_; x++) { + uint8_t *img_ptr = + (uint8_t *)img.data + + (v_ref_i + x * scale - patch_size_half_ * scale) * width_ + + u_ref_i - patch_size_half_ * scale; + for (int y = 0; y < patch_size_; ++y, img_ptr += scale) { + // 残差计算 + // 当前帧光度I_k + double cur_value = w_ref_tl * img_ptr[0] + w_ref_tr * img_ptr[scale] + + w_ref_bl * img_ptr[scale * width_] + + w_ref_br * img_ptr[scale * width_ + scale]; + // 残差计算 根据公式（22）,z = τ_k * I_k - τ_r * I_r + double res = + state_->inv_expo_time * cur_value - + inv_ref_expo * P[patch_size_total_ * level + x * patch_size_ + y]; + z(i * patch_size_total_ + x * patch_size_ + y) = res; + patch_error += res * res; + n_meas += 1; + visual_submap_->errors[i] = patch_error; + error += patch_error; - for (int x = 0; x < patch_size; x++) - { - uint8_t *img_ptr = (uint8_t *)img.data + (v_ref_i + x * scale - patch_size_half * scale) * width + u_ref_i - patch_size_half * scale; - for (int y = 0; y < patch_size; ++y, img_ptr += scale) - { + // 雅可比计算 + // 像素梯度计算，∂I/∂u float du = 0.5f * - ((w_ref_tl * img_ptr[scale] + w_ref_tr * img_ptr[scale * 2] + w_ref_bl * img_ptr[scale * width + scale] + - w_ref_br * img_ptr[scale * width + scale * 2]) - - (w_ref_tl * img_ptr[-scale] + w_ref_tr * img_ptr[0] + w_ref_bl * img_ptr[scale * width - scale] + w_ref_br * img_ptr[scale * width])); + ((w_ref_tl * img_ptr[scale] + w_ref_tr * img_ptr[scale * 2] + + w_ref_bl * img_ptr[scale * width_ + scale] + + w_ref_br * img_ptr[scale * width_ + scale * 2]) - + (w_ref_tl * img_ptr[-scale] + w_ref_tr * img_ptr[0] + + w_ref_bl * img_ptr[scale * width_ - scale] + + w_ref_br * img_ptr[scale * width_])); float dv = - 0.5f * - ((w_ref_tl * img_ptr[scale * width] + w_ref_tr * img_ptr[scale + scale * width] + w_ref_bl * img_ptr[width * scale * 2] + - w_ref_br * img_ptr[width * scale * 2 + scale]) - - (w_ref_tl * img_ptr[-scale * width] + w_ref_tr * img_ptr[-scale * width + scale] + w_ref_bl * img_ptr[0] + w_ref_br * img_ptr[scale])); + 0.5f * ((w_ref_tl * img_ptr[scale * width_] + + w_ref_tr * img_ptr[scale + scale * width_] + + w_ref_bl * img_ptr[width_ * scale * 2] + + w_ref_br * img_ptr[width_ * scale * 2 + scale]) - + (w_ref_tl * img_ptr[-scale * width_] + + w_ref_tr * img_ptr[-scale * width_ + scale] + + w_ref_bl * img_ptr[0] + w_ref_br * img_ptr[scale])); Jimg << du, dv; - Jimg = Jimg * state->inv_expo_time; + Jimg = Jimg * state_->inv_expo_time; Jimg = Jimg * inv_scale; + // J = - ∂I/∂u * ∂u/∂ẟξ (视觉SLAM14讲8.19) Jdphi = Jimg * Jdpi * p_hat; Jdp = -Jimg * Jdpi; - JdR = Jdphi * Jdphi_dR + Jdp * Jdp_dR; - Jdt = Jdp * Jdp_dt; - - double cur_value = - w_ref_tl * img_ptr[0] + w_ref_tr * img_ptr[scale] + w_ref_bl * img_ptr[scale * width] + w_ref_br * img_ptr[scale * width + scale]; - double res = state->inv_expo_time * cur_value - inv_ref_expo * P[patch_size_total * level + x * patch_size + y]; - - z(i * patch_size_total + x * patch_size + y) = res; - - patch_error += res * res; - n_meas += 1; - - if (exposure_estimate_en) { H_sub.block<1, 7>(i * patch_size_total + x * patch_size + y, 0) << JdR, Jdt, cur_value; } - else { H_sub.block<1, 6>(i * patch_size_total + x * patch_size + y, 0) << JdR, Jdt; } + JdR = Jdphi * Jdphi_dR_ + Jdp * Jdp_dR_; + Jdt = Jdp * Jdp_dt_; + + if (exposure_estimate_en_) { + H_sub.block<1, 7>(i * patch_size_total_ + x * patch_size_ + y, 0) + << JdR, + Jdt, cur_value; + } else { + H_sub.block<1, 6>(i * patch_size_total_ + x * patch_size_ + y, 0) + << JdR, + Jdt; + } } } - visual_submap->errors[i] = patch_error; - error += patch_error; } error = error / n_meas; - - compute_jacobian_time += omp_get_wtime() - t1; - // printf("\nPYRAMID LEVEL %i\n---------------\n", level); - // std::cout << "It. " << iteration - // << "\t last_error = " << last_error - // << "\t new_error = " << error - // << std::endl; + compute_jacobian_time_ += omp_get_wtime() - t1; double t3 = omp_get_wtime(); - if (error <= last_error) - { - old_state = (*state); + if (error <= last_error) { + old_state = (*state_); last_error = error; - // K = (H.transpose() / img_point_cov * H + state->cov.inverse()).inverse() * H.transpose() / img_point_cov; auto - // vec = (*state_propagat) - (*state); G = K*H; - // (*state) += (-K*z + vec - G*vec); - + // 和LIO基本一致，根据公式11更新 auto &&H_sub_T = H_sub.transpose(); - H_T_H.setZero(); - G.setZero(); - H_T_H.block<7, 7>(0, 0) = H_sub_T * H_sub; - MD(DIM_STATE, DIM_STATE) &&K_1 = (H_T_H + (state->cov / img_point_cov).inverse()).inverse(); + H_T_H_.setZero(); + G_.setZero(); + H_T_H_.block<7, 7>(0, 0) = H_sub_T * H_sub; + Mat19d &&K_1 = + (H_T_H_ + (state_->cov / img_point_cov_).inverse()).inverse(); auto &&HTz = H_sub_T * z; - // K = K_1.block(0,0) * H_sub_T; - auto vec = (*state_propagat) - (*state); - G.block(0, 0) = K_1.block(0, 0) * H_T_H.block<7, 7>(0, 0); + auto vec = (*state_propagat_) - (*state_); + G_.block(0, 0) = + K_1.block(0, 0) * H_T_H_.block<7, 7>(0, 0); MD(DIM_STATE, 1) - solution = -K_1.block(0, 0) * HTz + vec - G.block(0, 0) * vec.block<7, 1>(0, 0); + solution = -K_1.block(0, 0) * HTz + vec - + G_.block(0, 0) * vec.block<7, 1>(0, 0); - (*state) += solution; + (*state_) += solution; auto &&rot_add = solution.block<3, 1>(0, 0); auto &&t_add = solution.block<3, 1>(3, 0); auto &&expo_add = solution.block<1, 1>(6, 0); - // if ((rot_add.norm() * 57.3f < 0.001f) && (t_add.norm() * 100.0f < 0.001f) && (expo_add.norm() < 0.001f)) EKF_end = true; - if ((rot_add.norm() * 57.3f < 0.001f) && (t_add.norm() * 100.0f < 0.001f)) EKF_end = true; - } - else - { - (*state) = old_state; + // if ((rot_add.norm() * 57.3f < 0.001f) && (t_add.norm() * 100.0f < + // 0.001f) && (expo_add.norm() < 0.001f)) EKF_end = true; + if ((rot_add.norm() * 57.3f < 0.001f) && (t_add.norm() * 100.0f < 0.001f)) + EKF_end = true; + } else { + (*state_) = old_state; EKF_end = true; } - update_ekf_time += omp_get_wtime() - t3; + update_ekf_time_ += omp_get_wtime() - t3; - if (iteration == max_iterations || EKF_end) break; + if (iteration == max_iterations_ || EKF_end) break; } - // if (state->inv_expo_time < 0.0) {ROS_ERROR("reset expo time!!!!!!!!!!\n"); state->inv_expo_time = 0.0;} } -void VIOManager::updateFrameState(StatesGroup state) -{ +void VIOManager::UpdateFrameState(StatesGroup state) { M3D Rwi(state.rot_end); V3D Pwi(state.pos_end); - Rcw = Rci * Rwi.transpose(); - Pcw = -Rci * Rwi.transpose() * Pwi + Pci; - new_frame_->T_f_w_ = SE3(Rcw, Pcw); + Rcw_ = Rci_ * Rwi.transpose(); + Pcw_ = -Rci_ * Rwi.transpose() * Pwi + Pci_; + new_frame_->T_f_w_ = SE3(Mat3ToSO3(Rcw_), Pcw_); } -void VIOManager::plotTrackedPoints() -{ - int total_points = visual_submap->voxel_points.size(); +void VIOManager::PlotTrackedPoints() { + int total_points = visual_submap_->voxel_points.size(); if (total_points == 0) return; - // int inlier_count = 0; - // for (int i = 0; i < img_cp.rows / grid_size; i++) - // { - // cv::line(img_cp, cv::Poaint2f(0, grid_size * i), cv::Point2f(img_cp.cols, grid_size * i), cv::Scalar(255, 255, 255), 1, CV_AA); - // } - // for (int i = 0; i < img_cp.cols / grid_size; i++) - // { - // cv::line(img_cp, cv::Point2f(grid_size * i, 0), cv::Point2f(grid_size * i, img_cp.rows), cv::Scalar(255, 255, 255), 1, CV_AA); - // } - // for (int i = 0; i < img_cp.rows / grid_size; i++) - // { - // cv::line(img_cp, cv::Point2f(0, grid_size * i), cv::Point2f(img_cp.cols, grid_size * i), cv::Scalar(255, 255, 255), 1, CV_AA); - // } - // for (int i = 0; i < img_cp.cols / grid_size; i++) - // { - // cv::line(img_cp, cv::Point2f(grid_size * i, 0), cv::Point2f(grid_size * i, img_cp.rows), cv::Scalar(255, 255, 255), 1, CV_AA); - // } - for (int i = 0; i < total_points; i++) - { - VisualPoint *pt = visual_submap->voxel_points[i]; + + for (int i = 0; i < total_points; i++) { + VisualPoint *pt = visual_submap_->voxel_points[i]; V2D pc(new_frame_->w2c(pt->pos_)); - if (visual_submap->errors[i] <= visual_submap->propa_errors[i]) - { + if (visual_submap_->errors[i] <= visual_submap_->propa_errors[i]) { // inlier_count++; - cv::circle(img_cp, cv::Point2f(pc[0], pc[1]), 7, cv::Scalar(0, 255, 0), -1, 8); // Green Sparse Align tracked - } - else - { - cv::circle(img_cp, cv::Point2f(pc[0], pc[1]), 7, cv::Scalar(255, 0, 0), -1, 8); // Blue Sparse Align tracked + cv::circle(img_cp_, cv::Point2f(pc[0], pc[1]), 7, cv::Scalar(0, 255, 0), + -1, 8); // Green Sparse Align tracked + } else { + cv::circle(img_cp_, cv::Point2f(pc[0], pc[1]), 7, cv::Scalar(255, 0, 0), + -1, 8); // Blue Sparse Align tracked } } - // std::string text = std::to_string(inlier_count) + " " + std::to_string(total_points); - // cv::Point2f origin; - // origin.x = img_cp.cols - 110; - // origin.y = 20; - // cv::putText(img_cp, text, origin, cv::FONT_HERSHEY_COMPLEX, 0.7, cv::Scalar(0, 255, 0), 2, 8, 0); } -V3F VIOManager::getInterpolatedPixel(cv::Mat img, V2D pc) -{ +V3F VIOManager::GetInterpolatedPixel(cv::Mat img, V2D pc) { const float u_ref = pc[0]; const float v_ref = pc[1]; const int u_ref_i = floorf(pc[0]); @@ -1751,126 +2287,258 @@ V3F VIOManager::getInterpolatedPixel(cv::Mat img, V2D pc) const float w_ref_tr = subpix_u_ref * (1.0 - subpix_v_ref); const float w_ref_bl = (1.0 - subpix_u_ref) * subpix_v_ref; const float w_ref_br = subpix_u_ref * subpix_v_ref; - uint8_t *img_ptr = (uint8_t *)img.data + ((v_ref_i)*width + (u_ref_i)) * 3; - float B = w_ref_tl * img_ptr[0] + w_ref_tr * img_ptr[0 + 3] + w_ref_bl * img_ptr[width * 3] + w_ref_br * img_ptr[width * 3 + 0 + 3]; - float G = w_ref_tl * img_ptr[1] + w_ref_tr * img_ptr[1 + 3] + w_ref_bl * img_ptr[1 + width * 3] + w_ref_br * img_ptr[width * 3 + 1 + 3]; - float R = w_ref_tl * img_ptr[2] + w_ref_tr * img_ptr[2 + 3] + w_ref_bl * img_ptr[2 + width * 3] + w_ref_br * img_ptr[width * 3 + 2 + 3]; + uint8_t *img_ptr = (uint8_t *)img.data + ((v_ref_i)*width_ + (u_ref_i)) * 3; + float B = w_ref_tl * img_ptr[0] + w_ref_tr * img_ptr[0 + 3] + + w_ref_bl * img_ptr[width_ * 3] + + w_ref_br * img_ptr[width_ * 3 + 0 + 3]; + float G = w_ref_tl * img_ptr[1] + w_ref_tr * img_ptr[1 + 3] + + w_ref_bl * img_ptr[1 + width_ * 3] + + w_ref_br * img_ptr[width_ * 3 + 1 + 3]; + float R = w_ref_tl * img_ptr[2] + w_ref_tr * img_ptr[2 + 3] + + w_ref_bl * img_ptr[2 + width_ * 3] + + w_ref_br * img_ptr[width_ * 3 + 2 + 3]; V3F pixel(B, G, R); return pixel; } -void VIOManager::dumpDataForColmap() -{ +void VIOManager::DumpDataForColmap() { static int cnt = 1; std::ostringstream ss; ss << std::setw(5) << std::setfill('0') << cnt; std::string cnt_str = ss.str(); - std::string image_path = std::string(ROOT_DIR) + "Log/Colmap/images/" + cnt_str + ".png"; - + std::string image_path = + std::string(ROOT_DIR) + "Log/Colmap/images/" + cnt_str + ".png"; + cv::Mat img_rgb_undistort; - pinhole_cam->undistortImage(img_rgb, img_rgb_undistort); + pinhole_cam_->undistortImage(img_rgb_, img_rgb_undistort); cv::imwrite(image_path, img_rgb_undistort); - - Eigen::Quaterniond q(new_frame_->T_f_w_.rotation_matrix()); + + Eigen::Quaterniond q(new_frame_->T_f_w_.rotationMatrix()); Eigen::Vector3d t = new_frame_->T_f_w_.translation(); - fout_colmap << cnt << " " - << std::fixed << std::setprecision(6) // 保证浮点数精度为6位 - << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << " " - << t.x() << " " << t.y() << " " << t.z() << " " - << 1 << " " // CAMERA_ID (假设相机ID为1) - << cnt_str << ".png" << std::endl; - fout_colmap << "0.0 0.0 -1" << std::endl; + fout_colmap_ << cnt << " " << std::fixed + << std::setprecision(6) // 保证浮点数精度为6位 + << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << " " + << t.x() << " " << t.y() << " " << t.z() << " " << 1 + << " " // CAMERA_ID (假设相机ID为1) + << cnt_str << ".png" << std::endl; + fout_colmap_ << "0.0 0.0 -1" << std::endl; cnt++; } -void VIOManager::processFrame(cv::Mat &img, vector &pg, const unordered_map &feat_map, double img_time) -{ - if (width != img.cols || height != img.rows) - { +#if 0 +void VIOManager::ProcessFrame( + cv::Mat &img, std::vector &pv_list, + const std::unordered_map &voxel_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); + 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_test = img.clone(); + img_rgb_ = img.clone(); + img_cp_ = img.clone(); + // 转灰度图 if (img.channels() == 3) cv::cvtColor(img, img, CV_BGR2GRAY); + // 组建相机-图片帧 + new_frame_.reset(new Frame(cam_, img)); - new_frame_.reset(new Frame(cam, img)); - updateFrameState(*state); - - resetGrid(); + UpdateFrameState(*state_); - double t1 = omp_get_wtime(); + ResetGrid(); - retrieveFromVisualSparseMap(img, pg, feat_map); + double t1 = omp_get_wtime(); + // 提取当前帧的特征 + RetrieveFromVisualSparseMap(img, pv_list, voxel_map); double t2 = omp_get_wtime(); - computeJacobianAndUpdateEKF(img); + // 更新ESIEKF + ComputeJacobianAndUpdateEKF(img); double t3 = omp_get_wtime(); - generateVisualMapPoints(img, pg); + // 第一次执行时从此处开始，生成特征地图 + GenerateVisualMapPoints(img, pv_list); double t4 = omp_get_wtime(); - - plotTrackedPoints(); - if (plot_flag) projectPatchFromRefToCur(feat_map); + // 绘制被跟踪的特征点 + PlotTrackedPoints(); + + // 实际不执行 + if (plot_flag_) ProjectPatchFromRefToCur(); double t5 = omp_get_wtime(); - updateVisualMapPoints(img); + // 更新特征地图 + UpdateVisualMapPoints(img); double t6 = omp_get_wtime(); + // 更新相关图像块 + UpdateReferencePatch(voxel_map); + + double t7 = omp_get_wtime(); + + // 保存高斯泼溅需要的ColMap + if (colmap_output_en_) DumpDataForColmap(); + + frame_count_++; + ave_total_ = ave_total_ * (frame_count_ - 1) / frame_count_ + + (t7 - t1 - (t5 - t4)) / frame_count_; + + 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", + voxel_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"); +} +#endif + +void VIOManager::ProcessFrame( + cv::Mat &img, std::vector &pv_list, + const std::unordered_map::iterator> &vm_map) { + 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(); - updateReferencePatch(feat_map); + // 转灰度图 + if (img.channels() == 3) cv::cvtColor(img, img, CV_BGR2GRAY); + // 组建相机-图片帧 + new_frame_.reset(new ImageFrame(cam_, img)); + + UpdateFrameState(*state_); + + ResetGrid(); + + double t1 = omp_get_wtime(); + // 提取当前帧的特征 + RetrieveFromVisualSparseMapLRU(img, pv_list, vm_map); + + double t2 = omp_get_wtime(); + + // 更新ESIEKF + ComputeJacobianAndUpdateEKF(img); + + double t3 = omp_get_wtime(); + + // 第一次执行时从此处开始，生成特征地图 + GenerateVisualMapPoints(img, pv_list); + + double t4 = omp_get_wtime(); + + // 绘制被跟踪的特征点 + PlotTrackedPoints(); + + // 实际不执行 + if (plot_flag_) ProjectPatchFromRefToCur(); + + double t5 = omp_get_wtime(); + + // 更新特征地图 + UpdateVisualMapPoints(img); + + double t6 = omp_get_wtime(); + // 更新相关图像块 + UpdateReferencePatch(vm_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); + + // 保存高斯泼溅需要的ColMap + if (colmap_output_en_) DumpDataForColmap(); + + frame_count_++; + ave_total_ = ave_total_ * (frame_count_ - 1) / frame_count_ + + (t7 - t1 - (t5 - t4)) / frame_count_; + + 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", + vm_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"); } \ No newline at end of file diff --git a/src/visual_point.cpp b/src/visual_point.cpp deleted file mode 100644 index 1b746509..00000000 --- a/src/visual_point.cpp +++ /dev/null @@ -1,127 +0,0 @@ -/* -This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. - -Developer: Chunran Zheng - -For commercial use, please contact me at or -Prof. Fu Zhang at . - -This file is subject to the terms and conditions outlined in the 'LICENSE' file, -which is included as part of this source code package. -*/ - -#include "visual_point.h" -#include "feature.h" -#include -#include - -VisualPoint::VisualPoint(const Vector3d &pos) - : pos_(pos), previous_normal_(Vector3d::Zero()), normal_(Vector3d::Zero()), - is_converged_(false), is_normal_initialized_(false), has_ref_patch_(false) -{ -} - -VisualPoint::~VisualPoint() -{ - for (auto it = obs_.begin(), ite = obs_.end(); it != ite; ++it) - { - delete(*it); - } - obs_.clear(); - ref_patch = nullptr; -} - -void VisualPoint::addFrameRef(Feature *ftr) -{ - obs_.push_front(ftr); -} - -void VisualPoint::deleteFeatureRef(Feature *ftr) -{ - if (ref_patch == ftr) - { - ref_patch = nullptr; - has_ref_patch_ = false; - } - for (auto it = obs_.begin(), ite = obs_.end(); it != ite; ++it) - { - if ((*it) == ftr) - { - delete((*it)); - obs_.erase(it); - return; - } - } -} - -bool VisualPoint::getCloseViewObs(const Vector3d &framepos, Feature *&ftr, const Vector2d &cur_px) const -{ - // TODO: get frame with same point of view AND same pyramid level! - if (obs_.size() <= 0) return false; - - Vector3d obs_dir(framepos - pos_); - obs_dir.normalize(); - auto min_it = obs_.begin(); - double min_cos_angle = 0; - for (auto it = obs_.begin(), ite = obs_.end(); it != ite; ++it) - { - Vector3d dir((*it)->T_f_w_.inverse().translation() - pos_); - dir.normalize(); - double cos_angle = obs_dir.dot(dir); - if (cos_angle > min_cos_angle) - { - min_cos_angle = cos_angle; - min_it = it; - } - } - ftr = *min_it; - - // Vector2d ftr_px = ftr->px_; - // double pixel_dist = (cur_px-ftr_px).norm(); - - // if(pixel_dist > 200) - // { - // ROS_ERROR("The pixel dist exceeds 200."); - // return false; - // } - - if (min_cos_angle < 0.5) // assume that observations larger than 60° are useless 0.5 - { - // ROS_ERROR("The obseved angle is larger than 60°."); - return false; - } - - return true; -} - -void VisualPoint::findMinScoreFeature(const Vector3d &framepos, Feature *&ftr) const -{ - auto min_it = obs_.begin(); - float min_score = std::numeric_limits::max(); - - for (auto it = obs_.begin(), ite = obs_.end(); it != ite; ++it) - { - if ((*it)->score_ < min_score) - { - min_score = (*it)->score_; - min_it = it; - } - } - ftr = *min_it; -} - -void VisualPoint::deleteNonRefPatchFeatures() -{ - for (auto it = obs_.begin(); it != obs_.end();) - { - if (*it != ref_patch) - { - delete *it; - it = obs_.erase(it); - } - else - { - ++it; - } - } -} \ No newline at end of file diff --git a/src/voxel_map.cpp b/src/voxel_map.cpp index 01923cca..c4908c7e 100644 --- a/src/voxel_map.cpp +++ b/src/voxel_map.cpp @@ -1,4 +1,4 @@ -/* +/* This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. Developer: Chunran Zheng @@ -12,61 +12,70 @@ which is included as part of this source code package. #include "voxel_map.h" -void calcBodyCov(Eigen::Vector3d &pb, const float range_inc, const float degree_inc, Eigen::Matrix3d &cov) -{ +#include + +void CalcBodyCov(Eigen::Vector3d &pb, const float range_inc, + const float degree_inc, Eigen::Matrix3d &cov) { if (pb[2] == 0) pb[2] = 0.0001; float range = sqrt(pb[0] * pb[0] + pb[1] * pb[1] + pb[2] * pb[2]); float range_var = range_inc * range_inc; Eigen::Matrix2d direction_var; - direction_var << pow(sin(DEG2RAD(degree_inc)), 2), 0, 0, pow(sin(DEG2RAD(degree_inc)), 2); + direction_var << pow(sin(DEG2RAD(degree_inc)), 2), 0, 0, + pow(sin(DEG2RAD(degree_inc)), 2); Eigen::Vector3d direction(pb); direction.normalize(); Eigen::Matrix3d direction_hat; - direction_hat << 0, -direction(2), direction(1), direction(2), 0, -direction(0), -direction(1), direction(0), 0; - Eigen::Vector3d base_vector1(1, 1, -(direction(0) + direction(1)) / direction(2)); + direction_hat << 0, -direction(2), direction(1), direction(2), 0, + -direction(0), -direction(1), direction(0), 0; + Eigen::Vector3d base_vector1(1, 1, + -(direction(0) + direction(1)) / direction(2)); base_vector1.normalize(); Eigen::Vector3d base_vector2 = base_vector1.cross(direction); base_vector2.normalize(); Eigen::Matrix N; - N << base_vector1(0), base_vector2(0), base_vector1(1), base_vector2(1), base_vector1(2), base_vector2(2); + N << base_vector1(0), base_vector2(0), base_vector1(1), base_vector2(1), + base_vector1(2), base_vector2(2); Eigen::Matrix A = range * direction_hat * N; - cov = direction * range_var * direction.transpose() + A * direction_var * A.transpose(); + cov = direction * range_var * direction.transpose() + + A * direction_var * A.transpose(); } -void loadVoxelConfig(ros::NodeHandle &nh, VoxelMapConfig &voxel_config) -{ +void loadVoxelConfig(ros::NodeHandle &nh, VoxelMapConfig &voxel_config) { nh.param("publish/pub_plane_en", voxel_config.is_pub_plane_map_, false); - + nh.param("lio/max_layer", voxel_config.max_layer_, 1); nh.param("lio/voxel_size", voxel_config.max_voxel_size_, 0.5); - nh.param("lio/min_eigen_value", voxel_config.planner_threshold_, 0.01); + nh.param("lio/min_eigen_value", voxel_config.planner_threshold_, + 0.01); nh.param("lio/sigma_num", voxel_config.sigma_num_, 3); nh.param("lio/beam_err", voxel_config.beam_err_, 0.02); nh.param("lio/dept_err", voxel_config.dept_err_, 0.05); - nh.param>("lio/layer_init_num", voxel_config.layer_init_num_, vector{5,5,5,5,5}); + nh.param>("lio/layer_init_num", voxel_config.layer_init_num_, + std::vector{5, 5, 5, 5, 5}); nh.param("lio/max_points_num", voxel_config.max_points_num_, 50); nh.param("lio/max_iterations", voxel_config.max_iterations_, 5); - nh.param("local_map/map_sliding_en", voxel_config.map_sliding_en, false); + nh.param("local_map/map_sliding_en", voxel_config.map_sliding_en, + false); nh.param("local_map/half_map_size", voxel_config.half_map_size, 100); nh.param("local_map/sliding_thresh", voxel_config.sliding_thresh, 8); } -void VoxelOctoTree::init_plane(const std::vector &points, VoxelPlane *plane) -{ +void VoxelOctoTree::InitPlane(const std::vector &points, + VoxelPlane *plane) { plane->plane_var_ = Eigen::Matrix::Zero(); plane->covariance_ = Eigen::Matrix3d::Zero(); plane->center_ = Eigen::Vector3d::Zero(); plane->normal_ = Eigen::Vector3d::Zero(); plane->points_size_ = points.size(); plane->radius_ = 0; - for (auto pv : points) - { + for (auto pv : points) { plane->covariance_ += pv.point_w * pv.point_w.transpose(); plane->center_ += pv.point_w; } plane->center_ = plane->center_ / plane->points_size_; - plane->covariance_ = plane->covariance_ / plane->points_size_ - plane->center_ * plane->center_.transpose(); + plane->covariance_ = plane->covariance_ / plane->points_size_ - + plane->center_ * plane->center_.transpose(); Eigen::EigenSolver es(plane->covariance_); Eigen::Matrix3cd evecs = es.eigenvectors(); Eigen::Vector3cd evals = es.eigenvalues(); @@ -80,26 +89,23 @@ void VoxelOctoTree::init_plane(const std::vector &points, VoxelPla Eigen::Vector3d evecMid = evecs.real().col(evalsMid); Eigen::Vector3d evecMax = evecs.real().col(evalsMax); Eigen::Matrix3d J_Q; - J_Q << 1.0 / plane->points_size_, 0, 0, 0, 1.0 / plane->points_size_, 0, 0, 0, 1.0 / plane->points_size_; + J_Q << 1.0 / plane->points_size_, 0, 0, 0, 1.0 / plane->points_size_, 0, 0, 0, + 1.0 / plane->points_size_; // && evalsReal(evalsMid) > 0.05 //&& evalsReal(evalsMid) > 0.01 - if (evalsReal(evalsMin) < planer_threshold_) - { - for (int i = 0; i < points.size(); i++) - { + if (evalsReal(evalsMin) < planer_threshold_) { + for (int i = 0; i < points.size(); i++) { Eigen::Matrix J; Eigen::Matrix3d F; - for (int m = 0; m < 3; m++) - { - if (m != (int)evalsMin) - { + for (int m = 0; m < 3; m++) { + if (m != (int)evalsMin) { Eigen::Matrix F_m = - (points[i].point_w - plane->center_).transpose() / ((plane->points_size_) * (evalsReal[evalsMin] - evalsReal[m])) * - (evecs.real().col(m) * evecs.real().col(evalsMin).transpose() + evecs.real().col(evalsMin) * evecs.real().col(m).transpose()); + (points[i].point_w - plane->center_).transpose() / + ((plane->points_size_) * (evalsReal[evalsMin] - evalsReal[m])) * + (evecs.real().col(m) * evecs.real().col(evalsMin).transpose() + + evecs.real().col(evalsMin) * evecs.real().col(m).transpose()); F.row(m) = F_m; - } - else - { + } else { Eigen::Matrix F_m; F_m << 0, 0, 0; F.row(m) = F_m; @@ -110,104 +116,101 @@ void VoxelOctoTree::init_plane(const std::vector &points, VoxelPla plane->plane_var_ += J * points[i].var * J.transpose(); } - plane->normal_ << evecs.real()(0, evalsMin), evecs.real()(1, evalsMin), evecs.real()(2, evalsMin); - plane->y_normal_ << evecs.real()(0, evalsMid), evecs.real()(1, evalsMid), evecs.real()(2, evalsMid); - plane->x_normal_ << evecs.real()(0, evalsMax), evecs.real()(1, evalsMax), evecs.real()(2, evalsMax); + plane->normal_ << evecs.real()(0, evalsMin), evecs.real()(1, evalsMin), + evecs.real()(2, evalsMin); + plane->y_normal_ << evecs.real()(0, evalsMid), evecs.real()(1, evalsMid), + evecs.real()(2, evalsMid); + plane->x_normal_ << evecs.real()(0, evalsMax), evecs.real()(1, evalsMax), + evecs.real()(2, evalsMax); plane->min_eigen_value_ = evalsReal(evalsMin); plane->mid_eigen_value_ = evalsReal(evalsMid); plane->max_eigen_value_ = evalsReal(evalsMax); plane->radius_ = sqrt(evalsReal(evalsMax)); - plane->d_ = -(plane->normal_(0) * plane->center_(0) + plane->normal_(1) * plane->center_(1) + plane->normal_(2) * plane->center_(2)); + plane->d_ = -(plane->normal_(0) * plane->center_(0) + + plane->normal_(1) * plane->center_(1) + + plane->normal_(2) * plane->center_(2)); plane->is_plane_ = true; plane->is_update_ = true; - if (!plane->is_init_) - { + if (!plane->is_init_) { plane->id_ = voxel_plane_id; voxel_plane_id++; plane->is_init_ = true; } - } - else - { + } else { plane->is_update_ = true; plane->is_plane_ = false; } } -void VoxelOctoTree::init_octo_tree() -{ - if (temp_points_.size() > points_size_threshold_) - { - init_plane(temp_points_, plane_ptr_); - if (plane_ptr_->is_plane_ == true) - { +void VoxelOctoTree::InitOctoTree() { + if (temp_points_.size() > points_size_threshold_) { + InitPlane(temp_points_, plane_ptr_); + if (plane_ptr_->is_plane_ == true) { octo_state_ = 0; // new added - if (temp_points_.size() > max_points_num_) - { + if (temp_points_.size() > max_points_num_) { update_enable_ = false; std::vector().swap(temp_points_); new_points_ = 0; } - } - else - { + } else { octo_state_ = 1; - cut_octo_tree(); + CutOctoTree(); } init_octo_ = true; new_points_ = 0; } } -void VoxelOctoTree::cut_octo_tree() -{ - if (layer_ >= max_layer_) - { +void VoxelOctoTree::CutOctoTree() { + if (layer_ >= max_layer_) { octo_state_ = 0; return; } - for (size_t i = 0; i < temp_points_.size(); i++) - { + for (size_t i = 0; i < temp_points_.size(); i++) { int xyz[3] = {0, 0, 0}; - if (temp_points_[i].point_w[0] > voxel_center_[0]) { xyz[0] = 1; } - if (temp_points_[i].point_w[1] > voxel_center_[1]) { xyz[1] = 1; } - if (temp_points_[i].point_w[2] > voxel_center_[2]) { xyz[2] = 1; } + if (temp_points_[i].point_w[0] > voxel_center_[0]) { + xyz[0] = 1; + } + if (temp_points_[i].point_w[1] > voxel_center_[1]) { + xyz[1] = 1; + } + if (temp_points_[i].point_w[2] > voxel_center_[2]) { + xyz[2] = 1; + } int leafnum = 4 * xyz[0] + 2 * xyz[1] + xyz[2]; - if (leaves_[leafnum] == nullptr) - { - leaves_[leafnum] = new VoxelOctoTree(max_layer_, layer_ + 1, layer_init_num_[layer_ + 1], max_points_num_, planer_threshold_); + if (leaves_[leafnum] == nullptr) { + leaves_[leafnum] = + new VoxelOctoTree(max_layer_, layer_ + 1, layer_init_num_[layer_ + 1], + max_points_num_, planer_threshold_); leaves_[leafnum]->layer_init_num_ = layer_init_num_; - leaves_[leafnum]->voxel_center_[0] = voxel_center_[0] + (2 * xyz[0] - 1) * quater_length_; - leaves_[leafnum]->voxel_center_[1] = voxel_center_[1] + (2 * xyz[1] - 1) * quater_length_; - leaves_[leafnum]->voxel_center_[2] = voxel_center_[2] + (2 * xyz[2] - 1) * quater_length_; + leaves_[leafnum]->voxel_center_[0] = + voxel_center_[0] + (2 * xyz[0] - 1) * quater_length_; + leaves_[leafnum]->voxel_center_[1] = + voxel_center_[1] + (2 * xyz[1] - 1) * quater_length_; + leaves_[leafnum]->voxel_center_[2] = + voxel_center_[2] + (2 * xyz[2] - 1) * quater_length_; leaves_[leafnum]->quater_length_ = quater_length_ / 2; } leaves_[leafnum]->temp_points_.push_back(temp_points_[i]); leaves_[leafnum]->new_points_++; } - for (uint i = 0; i < 8; i++) - { - if (leaves_[i] != nullptr) - { - if (leaves_[i]->temp_points_.size() > leaves_[i]->points_size_threshold_) - { - init_plane(leaves_[i]->temp_points_, leaves_[i]->plane_ptr_); - if (leaves_[i]->plane_ptr_->is_plane_) - { + for (uint i = 0; i < 8; i++) { + if (leaves_[i] != nullptr) { + if (leaves_[i]->temp_points_.size() > + leaves_[i]->points_size_threshold_) { + InitPlane(leaves_[i]->temp_points_, leaves_[i]->plane_ptr_); + if (leaves_[i]->plane_ptr_->is_plane_) { leaves_[i]->octo_state_ = 0; // new added - if (leaves_[i]->temp_points_.size() > leaves_[i]->max_points_num_) - { + if (leaves_[i]->temp_points_.size() > leaves_[i]->max_points_num_) { leaves_[i]->update_enable_ = false; std::vector().swap(leaves_[i]->temp_points_); new_points_ = 0; } - } - else - { + } else { leaves_[i]->octo_state_ = 1; - leaves_[i]->cut_octo_tree(); + leaves_[i]->CutOctoTree(); } leaves_[i]->init_octo_ = true; leaves_[i]->new_points_ = 0; @@ -216,69 +219,66 @@ void VoxelOctoTree::cut_octo_tree() } } -void VoxelOctoTree::UpdateOctoTree(const pointWithVar &pv) -{ - if (!init_octo_) - { +void VoxelOctoTree::UpdateOctoTree(const pointWithVar &pv) { + if (!init_octo_) { new_points_++; temp_points_.push_back(pv); - if (temp_points_.size() > points_size_threshold_) { init_octo_tree(); } - } - else - { - if (plane_ptr_->is_plane_) - { - if (update_enable_) - { + if (temp_points_.size() > points_size_threshold_) { + InitOctoTree(); + } + } else { + if (plane_ptr_->is_plane_) { + if (update_enable_) { new_points_++; temp_points_.push_back(pv); - if (new_points_ > update_size_threshold_) - { - init_plane(temp_points_, plane_ptr_); + if (new_points_ > update_size_threshold_) { + InitPlane(temp_points_, plane_ptr_); new_points_ = 0; } - if (temp_points_.size() >= max_points_num_) - { + if (temp_points_.size() >= max_points_num_) { update_enable_ = false; std::vector().swap(temp_points_); new_points_ = 0; } } - } - else - { - if (layer_ < max_layer_) - { + } else { + if (layer_ < max_layer_) { int xyz[3] = {0, 0, 0}; - if (pv.point_w[0] > voxel_center_[0]) { xyz[0] = 1; } - if (pv.point_w[1] > voxel_center_[1]) { xyz[1] = 1; } - if (pv.point_w[2] > voxel_center_[2]) { xyz[2] = 1; } + if (pv.point_w[0] > voxel_center_[0]) { + xyz[0] = 1; + } + if (pv.point_w[1] > voxel_center_[1]) { + xyz[1] = 1; + } + if (pv.point_w[2] > voxel_center_[2]) { + xyz[2] = 1; + } int leafnum = 4 * xyz[0] + 2 * xyz[1] + xyz[2]; - if (leaves_[leafnum] != nullptr) { leaves_[leafnum]->UpdateOctoTree(pv); } - else - { - leaves_[leafnum] = new VoxelOctoTree(max_layer_, layer_ + 1, layer_init_num_[layer_ + 1], max_points_num_, planer_threshold_); + if (leaves_[leafnum] != nullptr) { + leaves_[leafnum]->UpdateOctoTree(pv); + } else { + leaves_[leafnum] = new VoxelOctoTree( + max_layer_, layer_ + 1, layer_init_num_[layer_ + 1], + max_points_num_, planer_threshold_); leaves_[leafnum]->layer_init_num_ = layer_init_num_; - leaves_[leafnum]->voxel_center_[0] = voxel_center_[0] + (2 * xyz[0] - 1) * quater_length_; - leaves_[leafnum]->voxel_center_[1] = voxel_center_[1] + (2 * xyz[1] - 1) * quater_length_; - leaves_[leafnum]->voxel_center_[2] = voxel_center_[2] + (2 * xyz[2] - 1) * quater_length_; + leaves_[leafnum]->voxel_center_[0] = + voxel_center_[0] + (2 * xyz[0] - 1) * quater_length_; + leaves_[leafnum]->voxel_center_[1] = + voxel_center_[1] + (2 * xyz[1] - 1) * quater_length_; + leaves_[leafnum]->voxel_center_[2] = + voxel_center_[2] + (2 * xyz[2] - 1) * quater_length_; leaves_[leafnum]->quater_length_ = quater_length_ / 2; leaves_[leafnum]->UpdateOctoTree(pv); } - } - else - { - if (update_enable_) - { + } else { + if (update_enable_) { new_points_++; temp_points_.push_back(pv); - if (new_points_ > update_size_threshold_) - { - init_plane(temp_points_, plane_ptr_); + if (new_points_ > update_size_threshold_) { + InitPlane(temp_points_, plane_ptr_); new_points_ = 0; } - if (temp_points_.size() > max_points_num_) - { + if (temp_points_.size() > max_points_num_) { update_enable_ = false; std::vector().swap(temp_points_); new_points_ = 0; @@ -289,9 +289,9 @@ void VoxelOctoTree::UpdateOctoTree(const pointWithVar &pv) } } -VoxelOctoTree *VoxelOctoTree::find_correspond(Eigen::Vector3d pw) -{ - if (!init_octo_ || plane_ptr_->is_plane_ || (layer_ >= max_layer_)) return this; +VoxelOctoTree *VoxelOctoTree::FindCorrespond(Eigen::Vector3d pw) { + if (!init_octo_ || plane_ptr_->is_plane_ || (layer_ >= max_layer_)) + return this; int xyz[3] = {0, 0, 0}; xyz[0] = pw[0] > voxel_center_[0] ? 1 : 0; @@ -301,33 +301,37 @@ VoxelOctoTree *VoxelOctoTree::find_correspond(Eigen::Vector3d pw) // printf("leafnum: %d. \n", leafnum); - return (leaves_[leafnum] != nullptr) ? leaves_[leafnum]->find_correspond(pw) : this; + return (leaves_[leafnum] != nullptr) ? leaves_[leafnum]->FindCorrespond(pw) + : this; } -VoxelOctoTree *VoxelOctoTree::Insert(const pointWithVar &pv) -{ - if ((!init_octo_) || (init_octo_ && plane_ptr_->is_plane_) || (init_octo_ && (!plane_ptr_->is_plane_) && (layer_ >= max_layer_))) - { +VoxelOctoTree *VoxelOctoTree::Insert(const pointWithVar &pv) { + if ((!init_octo_) || (init_octo_ && plane_ptr_->is_plane_) || + (init_octo_ && (!plane_ptr_->is_plane_) && (layer_ >= max_layer_))) { new_points_++; temp_points_.push_back(pv); return this; } - if (init_octo_ && (!plane_ptr_->is_plane_) && (layer_ < max_layer_)) - { + if (init_octo_ && (!plane_ptr_->is_plane_) && (layer_ < max_layer_)) { int xyz[3] = {0, 0, 0}; xyz[0] = pv.point_w[0] > voxel_center_[0] ? 1 : 0; xyz[1] = pv.point_w[1] > voxel_center_[1] ? 1 : 0; xyz[2] = pv.point_w[2] > voxel_center_[2] ? 1 : 0; int leafnum = 4 * xyz[0] + 2 * xyz[1] + xyz[2]; - if (leaves_[leafnum] != nullptr) { return leaves_[leafnum]->Insert(pv); } - else - { - leaves_[leafnum] = new VoxelOctoTree(max_layer_, layer_ + 1, layer_init_num_[layer_ + 1], max_points_num_, planer_threshold_); + if (leaves_[leafnum] != nullptr) { + return leaves_[leafnum]->Insert(pv); + } else { + leaves_[leafnum] = + new VoxelOctoTree(max_layer_, layer_ + 1, layer_init_num_[layer_ + 1], + max_points_num_, planer_threshold_); leaves_[leafnum]->layer_init_num_ = layer_init_num_; - leaves_[leafnum]->voxel_center_[0] = voxel_center_[0] + (2 * xyz[0] - 1) * quater_length_; - leaves_[leafnum]->voxel_center_[1] = voxel_center_[1] + (2 * xyz[1] - 1) * quater_length_; - leaves_[leafnum]->voxel_center_[2] = voxel_center_[2] + (2 * xyz[2] - 1) * quater_length_; + leaves_[leafnum]->voxel_center_[0] = + voxel_center_[0] + (2 * xyz[0] - 1) * quater_length_; + leaves_[leafnum]->voxel_center_[1] = + voxel_center_[1] + (2 * xyz[1] - 1) * quater_length_; + leaves_[leafnum]->voxel_center_[2] = + voxel_center_[2] + (2 * xyz[2] - 1) * quater_length_; leaves_[leafnum]->quater_length_ = quater_length_ / 2; return leaves_[leafnum]->Insert(pv); } @@ -335,23 +339,28 @@ VoxelOctoTree *VoxelOctoTree::Insert(const pointWithVar &pv) return nullptr; } -void VoxelMapManager::StateEstimation(StatesGroup &state_propagat) -{ +VoxelMapManager::VoxelMapManager(VoxelMapConfig &config_setting) + : config_setting_(config_setting) { + current_frame_id_ = 0; + feats_down_body_.reset(new PointCloudXYZIN()); +}; + +void VoxelMapManager::StateEstimation(StatesGroup &state_propagat) { cross_mat_list_.clear(); cross_mat_list_.reserve(feats_down_size_); body_cov_list_.clear(); body_cov_list_.reserve(feats_down_size_); - // build_residual_time = 0.0; - // ekf_time = 0.0; - // double t0 = omp_get_wtime(); - - for (size_t i = 0; i < feats_down_body_->size(); i++) - { - V3D point_this(feats_down_body_->points[i].x, feats_down_body_->points[i].y, feats_down_body_->points[i].z); - if (point_this[2] == 0) { point_this[2] = 0.001; } + // 提前计算body_cov_list_和cross_mat_list_ + for (size_t i = 0; i < feats_down_body_->size(); i++) { + V3D point_this(feats_down_body_->points[i].x, feats_down_body_->points[i].y, + feats_down_body_->points[i].z); + if (point_this[2] == 0) { + point_this[2] = 0.001; + } M3D var; - calcBodyCov(point_this, config_setting_.dept_err_, config_setting_.beam_err_, var); + CalcBodyCov(point_this, config_setting_.dept_err_, + config_setting_.beam_err_, var); body_cov_list_.push_back(var); point_this = extR_ * point_this + extT_; M3D point_crossmat; @@ -359,60 +368,65 @@ void VoxelMapManager::StateEstimation(StatesGroup &state_propagat) cross_mat_list_.push_back(point_crossmat); } - vector().swap(pv_list_); + std::vector().swap(pv_list_); pv_list_.resize(feats_down_size_); int rematch_num = 0; - MD(DIM_STATE, DIM_STATE) G, H_T_H, I_STATE; + Mat19d G, H_T_H, I_STATE; G.setZero(); H_T_H.setZero(); I_STATE.setIdentity(); bool flg_EKF_inited, flg_EKF_converged, EKF_stop_flg = 0; - for (int iterCount = 0; iterCount < config_setting_.max_iterations_; iterCount++) - { + // 迭代计算 + for (int iterCount = 0; iterCount < config_setting_.max_iterations_; + iterCount++) { double total_residual = 0.0; - pcl::PointCloud::Ptr world_lidar(new pcl::PointCloud); - TransformLidar(state_.rot_end, state_.pos_end, feats_down_body_, world_lidar); + pcl::PointCloud::Ptr world_lidar( + new pcl::PointCloud); + TransformLidar(state_.rot_end, state_.pos_end, feats_down_body_, + world_lidar); M3D rot_var = state_.cov.block<3, 3>(0, 0); M3D t_var = state_.cov.block<3, 3>(3, 3); - for (size_t i = 0; i < feats_down_body_->size(); i++) - { + + for (size_t i = 0; i < feats_down_body_->size(); i++) { pointWithVar &pv = pv_list_[i]; - pv.point_b << feats_down_body_->points[i].x, feats_down_body_->points[i].y, feats_down_body_->points[i].z; - pv.point_w << world_lidar->points[i].x, world_lidar->points[i].y, world_lidar->points[i].z; + pv.point_b << feats_down_body_->points[i].x, + feats_down_body_->points[i].y, feats_down_body_->points[i].z; + pv.point_w << world_lidar->points[i].x, world_lidar->points[i].y, + world_lidar->points[i].z; M3D cov = body_cov_list_[i]; M3D point_crossmat = cross_mat_list_[i]; - cov = state_.rot_end * cov * state_.rot_end.transpose() + (-point_crossmat) * rot_var * (-point_crossmat.transpose()) + t_var; + cov = state_.rot_end * cov * state_.rot_end.transpose() + + (-point_crossmat) * rot_var * (-point_crossmat.transpose()) + t_var; pv.var = cov; pv.body_var = body_cov_list_[i]; } ptpl_list_.clear(); - // double t1 = omp_get_wtime(); - - BuildResidualListOMP(pv_list_, ptpl_list_); + // BuildResidualListOMP(pv_list_, ptpl_list_); + BuildResidualListLRU(pv_list_, ptpl_list_); - // build_residual_time += omp_get_wtime() - t1; - - for (int i = 0; i < ptpl_list_.size(); i++) - { + for (int i = 0; i < ptpl_list_.size(); i++) { total_residual += fabs(ptpl_list_[i].dis_to_plane_); } effct_feat_num_ = ptpl_list_.size(); - cout << "[ LIO ] Raw feature num: " << feats_undistort_->size() << ", downsampled feature num:" << feats_down_size_ - << " effective feature num: " << effct_feat_num_ << " average residual: " << total_residual / effct_feat_num_ << endl; + std::cout << "[ LIO ] Raw feature num: " << undistort_size_ + << ", downsampled feature num:" << feats_down_size_ + << " effective feature num: " << effct_feat_num_ + << " average residual: " << total_residual / effct_feat_num_ + << std::endl; /*** Computation of Measuremnt Jacobian matrix H and measurents covarience * ***/ - MatrixXd Hsub(effct_feat_num_, 6); - MatrixXd Hsub_T_R_inv(6, effct_feat_num_); - VectorXd R_inv(effct_feat_num_); - VectorXd meas_vec(effct_feat_num_); + Eigen::MatrixXd Hsub(effct_feat_num_, 6); + Eigen::MatrixXd Hsub_T_R_inv(6, effct_feat_num_); + Eigen::VectorXd R_inv(effct_feat_num_); + Eigen::VectorXd meas_vec(effct_feat_num_); meas_vec.setZero(); - for (int i = 0; i < effct_feat_num_; i++) - { + + for (int i = 0; i < effct_feat_num_; i++) { auto &ptpl = ptpl_list_[i]; V3D point_this(ptpl.point_b_); point_this = extR_ * point_this + extT_; @@ -422,7 +436,8 @@ void VoxelMapManager::StateEstimation(StatesGroup &state_propagat) /*** get the normal vector of closest surface/corner ***/ - V3D point_world = state_propagat.rot_end * point_this + state_propagat.pos_end; + V3D point_world = + state_propagat.rot_end * point_this + state_propagat.pos_end; Eigen::Matrix J_nq; J_nq.block<1, 3>(0, 0) = point_world - ptpl_list_[i].center_; J_nq.block<1, 3>(0, 3) = -ptpl_list_[i].normal_; @@ -431,92 +446,106 @@ void VoxelMapManager::StateEstimation(StatesGroup &state_propagat) // V3D normal_b = state_.rot_end.inverse() * ptpl_list_[i].normal_; // V3D point_b = ptpl_list_[i].point_b_; // double cos_theta = fabs(normal_b.dot(point_b) / point_b.norm()); - // ptpl_list_[i].body_cov_ = ptpl_list_[i].body_cov_ * (1.0 / cos_theta) * (1.0 / cos_theta); + // ptpl_list_[i].body_cov_ = ptpl_list_[i].body_cov_ * (1.0 / cos_theta) * + // (1.0 / cos_theta); // point_w cov - // var = state_propagat.rot_end * extR_ * ptpl_list_[i].body_cov_ * (state_propagat.rot_end * extR_).transpose() + - // state_propagat.cov.block<3, 3>(3, 3) + (-point_crossmat) * state_propagat.cov.block<3, 3>(0, 0) * (-point_crossmat).transpose(); + // var = state_propagat.rot_end * extR_ * ptpl_list_[i].body_cov_ * + // (state_propagat.rot_end * extR_).transpose() + + // state_propagat.cov.block<3, 3>(3, 3) + (-point_crossmat) * + // state_propagat.cov.block<3, 3>(0, 0) * + // (-point_crossmat).transpose(); // point_w cov (another_version) - // var = state_propagat.rot_end * extR_ * ptpl_list_[i].body_cov_ * (state_propagat.rot_end * extR_).transpose() + - // state_propagat.cov.block<3, 3>(3, 3) - point_crossmat * state_propagat.cov.block<3, 3>(0, 0) * point_crossmat; + // var = state_propagat.rot_end * extR_ * ptpl_list_[i].body_cov_ * + // (state_propagat.rot_end * extR_).transpose() + + // state_propagat.cov.block<3, 3>(3, 3) - point_crossmat * + // state_propagat.cov.block<3, 3>(0, 0) * point_crossmat; // point_body cov - var = state_propagat.rot_end * extR_ * ptpl_list_[i].body_cov_ * (state_propagat.rot_end * extR_).transpose(); + var = state_propagat.rot_end * extR_ * ptpl_list_[i].body_cov_ * + (state_propagat.rot_end * extR_).transpose(); double sigma_l = J_nq * ptpl_list_[i].plane_var_ * J_nq.transpose(); - R_inv(i) = 1.0 / (0.001 + sigma_l + ptpl_list_[i].normal_.transpose() * var * ptpl_list_[i].normal_); - // R_inv(i) = 1.0 / (sigma_l + ptpl_list_[i].normal_.transpose() * var * ptpl_list_[i].normal_); - - /*** calculate the Measuremnt Jacobian matrix H ***/ - V3D A(point_crossmat * state_.rot_end.transpose() * ptpl_list_[i].normal_); - Hsub.row(i) << VEC_FROM_ARRAY(A), ptpl_list_[i].normal_[0], ptpl_list_[i].normal_[1], ptpl_list_[i].normal_[2]; - Hsub_T_R_inv.col(i) << A[0] * R_inv(i), A[1] * R_inv(i), A[2] * R_inv(i), ptpl_list_[i].normal_[0] * R_inv(i), - ptpl_list_[i].normal_[1] * R_inv(i), ptpl_list_[i].normal_[2] * R_inv(i); + R_inv(i) = 1.0 / (0.001 + sigma_l + + ptpl_list_[i].normal_.transpose() * var * + ptpl_list_[i].normal_); + + // 计算海塞矩阵和残差 + V3D A(point_crossmat * state_.rot_end.transpose() * + ptpl_list_[i].normal_); + Hsub.row(i) << VEC_FROM_ARRAY(A), ptpl_list_[i].normal_[0], + ptpl_list_[i].normal_[1], ptpl_list_[i].normal_[2]; + Hsub_T_R_inv.col(i) << A[0] * R_inv(i), A[1] * R_inv(i), A[2] * R_inv(i), + ptpl_list_[i].normal_[0] * R_inv(i), + ptpl_list_[i].normal_[1] * R_inv(i), + ptpl_list_[i].normal_[2] * R_inv(i); + // 此处残差z添加上负号 meas_vec(i) = -ptpl_list_[i].dis_to_plane_; } EKF_stop_flg = false; flg_EKF_converged = false; - /*** Iterative Kalman Filter Update ***/ - MatrixXd K(DIM_STATE, effct_feat_num_); - // auto &&Hsub_T = Hsub.transpose(); + + // IESKF更新 + // 公式11其1 K = (H^T * R^-1 * H + P)^-1 * H^T * R^-1 + // HTz为 H^T * R^-1 * -z auto &&HTz = Hsub_T_R_inv * meas_vec; - // fout_dbg<<"HTz: "<(0, 0) = Hsub_T_R_inv * Hsub; - // EigenSolver> es(H_T_H.block<6,6>(0,0)); - MD(DIM_STATE, DIM_STATE) &&K_1 = (H_T_H.block(0, 0) + state_.cov.block(0, 0).inverse()).inverse(); - G.block(0, 0) = K_1.block(0, 0) * H_T_H.block<6, 6>(0, 0); + // K1为(H^T * R^-1 * H + P)^-1 + Mat19d &&K_1 = (H_T_H + state_.cov.inverse()).inverse(); + // G为K * H + G.block(0, 0) = + K_1.block(0, 0) * H_T_H.block<6, 6>(0, 0); + // 公式11其2 x_(k+1) = x_k ⊞ dx + // dx = -K * z - (I - K * H)(x_k ⊟ x_predict) + // vec 为 (x_predict ⊟ x_k) auto vec = state_propagat - state_; - VD(DIM_STATE) - solution = K_1.block(0, 0) * HTz + vec.block(0, 0) - G.block(0, 0) * vec.block<6, 1>(0, 0); - int minRow, minCol; + // solution为dx + // dx = -K * z - (x_k ⊟ x_predict) + K * H * (x_k ⊟ x_predict) + // dx = -K * z + (x_predict ⊟ x_k) - K * H * (x_predict ⊟ x_k) + Vec19d solution = K_1.block(0, 0) * HTz + + vec.block(0, 0) - + G.block(0, 0) * vec.block<6, 1>(0, 0); + // x_(k+1) = x_k ⊞ dx state_ += solution; auto rot_add = solution.block<3, 1>(0, 0); auto t_add = solution.block<3, 1>(3, 0); - if ((rot_add.norm() * 57.3 < 0.01) && (t_add.norm() * 100 < 0.015)) { flg_EKF_converged = true; } + if ((rot_add.norm() * 57.3 < 0.01) && (t_add.norm() * 100 < 0.015)) { + flg_EKF_converged = true; + } V3D euler_cur = state_.rot_end.eulerAngles(2, 1, 0); /*** Rematch Judgement ***/ - if (flg_EKF_converged || ((rematch_num == 0) && (iterCount == (config_setting_.max_iterations_ - 2)))) { rematch_num++; } + if (flg_EKF_converged || + ((rematch_num == 0) && + (iterCount == (config_setting_.max_iterations_ - 2)))) { + rematch_num++; + } /*** Convergence Judgements and Covariance Update ***/ - if (!EKF_stop_flg && (rematch_num >= 2 || (iterCount == config_setting_.max_iterations_ - 1))) - { + if (!EKF_stop_flg && (rematch_num >= 2 || + (iterCount == config_setting_.max_iterations_ - 1))) { /*** Covariance Update ***/ - // _state.cov = (I_STATE - G) * _state.cov; - state_.cov.block(0, 0) = - (I_STATE.block(0, 0) - G.block(0, 0)) * state_.cov.block(0, 0); - // total_distance += (_state.pos_end - position_last).norm(); + // P_k+1 = (I - KH)P_k + state_.cov = (I_STATE - G) * state_.cov; position_last_ = state_.pos_end; - geoQuat_ = tf::createQuaternionMsgFromRollPitchYaw(euler_cur(0), euler_cur(1), euler_cur(2)); - // VD(DIM_STATE) K_sum = K.rowwise().sum(); - // VD(DIM_STATE) P_diag = _state.cov.diagonal(); EKF_stop_flg = true; } if (EKF_stop_flg) break; } - - // double t2 = omp_get_wtime(); - // scan_count++; - // ekf_time = t2 - t0 - build_residual_time; - - // ave_build_residual_time = ave_build_residual_time * (scan_count - 1) / scan_count + build_residual_time / scan_count; - // ave_ekf_time = ave_ekf_time * (scan_count - 1) / scan_count + ekf_time / scan_count; - - // cout << "[ Mapping ] ekf_time: " << ekf_time << "s, build_residual_time: " << build_residual_time << "s" << endl; - // cout << "[ Mapping ] ave_ekf_time: " << ave_ekf_time << "s, ave_build_residual_time: " << ave_build_residual_time << "s" << endl; } -void VoxelMapManager::TransformLidar(const Eigen::Matrix3d rot, const Eigen::Vector3d t, const PointCloudXYZI::Ptr &input_cloud, - pcl::PointCloud::Ptr &trans_cloud) -{ +void VoxelMapManager::TransformLidar( + const Eigen::Matrix3d rot, const Eigen::Vector3d t, + const PointCloudXYZIN::Ptr &input_cloud, + pcl::PointCloud::Ptr &trans_cloud) { pcl::PointCloud().swap(*trans_cloud); trans_cloud->reserve(input_cloud->size()); - for (size_t i = 0; i < input_cloud->size(); i++) - { + 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); @@ -529,8 +558,8 @@ void VoxelMapManager::TransformLidar(const Eigen::Matrix3d rot, const Eigen::Vec } } -void VoxelMapManager::BuildVoxelMap() -{ +#if 0 +void VoxelMapManager::BuildVoxelMap() { float voxel_size = config_setting_.max_voxel_size_; float planer_threshold = config_setting_.planner_threshold_; int max_layer = config_setting_.max_layer_; @@ -539,41 +568,45 @@ void VoxelMapManager::BuildVoxelMap() std::vector input_points; - for (size_t i = 0; i < feats_down_world_->size(); i++) - { + for (size_t i = 0; i < feats_down_world_->size(); i++) { pointWithVar pv; - pv.point_w << feats_down_world_->points[i].x, feats_down_world_->points[i].y, feats_down_world_->points[i].z; - V3D point_this(feats_down_body_->points[i].x, feats_down_body_->points[i].y, feats_down_body_->points[i].z); + pv.point_w << feats_down_world_->points[i].x, + feats_down_world_->points[i].y, feats_down_world_->points[i].z; + V3D point_this(feats_down_body_->points[i].x, feats_down_body_->points[i].y, + feats_down_body_->points[i].z); M3D var; - calcBodyCov(point_this, config_setting_.dept_err_, config_setting_.beam_err_, var); + CalcBodyCov(point_this, config_setting_.dept_err_, + config_setting_.beam_err_, var); M3D point_crossmat; point_crossmat << SKEW_SYM_MATRX(point_this); - 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); + 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); pv.var = var; input_points.push_back(pv); } uint plsize = input_points.size(); - for (uint i = 0; i < plsize; i++) - { + for (uint i = 0; i < plsize; i++) { const pointWithVar p_v = input_points[i]; float loc_xyz[3]; - for (int j = 0; j < 3; j++) - { + for (int j = 0; j < 3; j++) { loc_xyz[j] = p_v.point_w[j] / voxel_size; - if (loc_xyz[j] < 0) { loc_xyz[j] -= 1.0; } + if (loc_xyz[j] < 0) { + loc_xyz[j] -= 1.0; + } } - VOXEL_LOCATION position((int64_t)loc_xyz[0], (int64_t)loc_xyz[1], (int64_t)loc_xyz[2]); + VOXEL_LOCATION position((int64_t)loc_xyz[0], (int64_t)loc_xyz[1], + (int64_t)loc_xyz[2]); auto iter = voxel_map_.find(position); - if (iter != voxel_map_.end()) - { + if (iter != voxel_map_.end()) { voxel_map_[position]->temp_points_.push_back(p_v); voxel_map_[position]->new_points_++; - } - else - { - VoxelOctoTree *octo_tree = new VoxelOctoTree(max_layer, 0, layer_init_num[0], max_points_num, planer_threshold); + } else { + VoxelOctoTree *octo_tree = new VoxelOctoTree( + max_layer, 0, layer_init_num[0], max_points_num, planer_threshold); voxel_map_[position] = octo_tree; voxel_map_[position]->quater_length_ = voxel_size / 4; voxel_map_[position]->voxel_center_[0] = (0.5 + position.x) * voxel_size; @@ -584,21 +617,100 @@ void VoxelMapManager::BuildVoxelMap() voxel_map_[position]->layer_init_num_ = layer_init_num; } } - for (auto iter = voxel_map_.begin(); iter != voxel_map_.end(); ++iter) - { - iter->second->init_octo_tree(); + for (auto iter = voxel_map_.begin(); iter != voxel_map_.end(); ++iter) { + iter->second->InitOctoTree(); } } +#endif -V3F VoxelMapManager::RGBFromVoxel(const V3D &input_point) -{ +void VoxelMapManager::BuildVoxelMapLRU( + const PointCloudXYZIN::Ptr &cloud_world) { + float voxel_size = config_setting_.max_voxel_size_; + float planer_threshold = config_setting_.planner_threshold_; + int max_layer = config_setting_.max_layer_; + int max_points_num = config_setting_.max_points_num_; + std::vector layer_init_num = config_setting_.layer_init_num_; + + std::vector input_points; + for (size_t i = 0; i < cloud_world->size(); i++) { + pointWithVar pv; + pv.point_w << cloud_world->points[i].x, cloud_world->points[i].y, + cloud_world->points[i].z; + V3D point_this(feats_down_body_->points[i].x, feats_down_body_->points[i].y, + feats_down_body_->points[i].z); + M3D var; + CalcBodyCov(point_this, config_setting_.dept_err_, + config_setting_.beam_err_, var); + M3D point_crossmat; + point_crossmat << SKEW_SYM_MATRX(point_this); + 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); + pv.var = var; + input_points.push_back(pv); + } + uint plsize = input_points.size(); + for (uint i = 0; i < plsize; i++) { + const pointWithVar p_v = input_points[i]; + float loc_xyz[3]; + for (int j = 0; j < 3; j++) { + loc_xyz[j] = p_v.point_w[j] / voxel_size; + if (loc_xyz[j] < 0) { + loc_xyz[j] -= 1.0; + } + } + VOXEL_LOCATION position((int64_t)loc_xyz[0], (int64_t)loc_xyz[1], + (int64_t)loc_xyz[2]); + auto iter = vm_map_.find(position); + if (iter != vm_map_.end()) { + // 体素已存在 + vm_map_[position]->second->temp_points_.push_back(p_v); + vm_map_[position]->second->new_points_++; + // 更新的放至最前 + vm_data_.splice(vm_data_.begin(), vm_data_, iter->second); + iter->second = vm_data_.begin(); + } else { + // 体素不存在 + VoxelOctoTree *octo_tree = new VoxelOctoTree( + max_layer, 0, layer_init_num[0], max_points_num, planer_threshold); + vm_data_.push_front({position, {octo_tree}}); + vm_map_.insert({position, vm_data_.begin()}); + vm_map_[position]->second->quater_length_ = voxel_size / 4; + vm_map_[position]->second->voxel_center_[0] = + (0.5 + position.x) * voxel_size; + vm_map_[position]->second->voxel_center_[1] = + (0.5 + position.y) * voxel_size; + vm_map_[position]->second->voxel_center_[2] = + (0.5 + position.z) * voxel_size; + vm_map_[position]->second->temp_points_.push_back(p_v); + vm_map_[position]->second->new_points_++; + vm_map_[position]->second->layer_init_num_ = layer_init_num; + + // LRU + if (vm_data_.size() >= lru_size_) { + // 删除一个尾部的数据 + vm_map_.erase(vm_data_.back().first); + delete vm_data_.back().second; + vm_data_.pop_back(); + } + } + } + + for (auto iter = vm_map_.begin(); iter != vm_map_.end(); ++iter) { + iter->second->second->InitOctoTree(); + } +} + +V3F VoxelMapManager::RGBFromVoxel(const V3D &input_point) { int64_t loc_xyz[3]; - for (int j = 0; j < 3; j++) - { + for (int j = 0; j < 3; j++) { loc_xyz[j] = floor(input_point[j] / config_setting_.max_voxel_size_); } - VOXEL_LOCATION position((int64_t)loc_xyz[0], (int64_t)loc_xyz[1], (int64_t)loc_xyz[2]); + VOXEL_LOCATION position((int64_t)loc_xyz[0], (int64_t)loc_xyz[1], + (int64_t)loc_xyz[2]); int64_t ind = loc_xyz[0] + loc_xyz[1] + loc_xyz[2]; uint k((ind + 100000) % 3); V3F RGB((k == 0) * 255.0, (k == 1) * 255.0, (k == 2) * 255.0); @@ -606,29 +718,32 @@ V3F VoxelMapManager::RGBFromVoxel(const V3D &input_point) return RGB; } -void VoxelMapManager::UpdateVoxelMap(const std::vector &input_points) -{ +#if 0 +void VoxelMapManager::UpdateVoxelMap( + const std::vector &input_points) { float voxel_size = config_setting_.max_voxel_size_; float planer_threshold = config_setting_.planner_threshold_; int max_layer = config_setting_.max_layer_; int max_points_num = config_setting_.max_points_num_; std::vector layer_init_num = config_setting_.layer_init_num_; uint plsize = input_points.size(); - for (uint i = 0; i < plsize; i++) - { + for (uint i = 0; i < plsize; i++) { const pointWithVar p_v = input_points[i]; float loc_xyz[3]; - for (int j = 0; j < 3; j++) - { + for (int j = 0; j < 3; j++) { loc_xyz[j] = p_v.point_w[j] / voxel_size; - if (loc_xyz[j] < 0) { loc_xyz[j] -= 1.0; } + if (loc_xyz[j] < 0) { + loc_xyz[j] -= 1.0; + } } - VOXEL_LOCATION position((int64_t)loc_xyz[0], (int64_t)loc_xyz[1], (int64_t)loc_xyz[2]); + VOXEL_LOCATION position((int64_t)loc_xyz[0], (int64_t)loc_xyz[1], + (int64_t)loc_xyz[2]); auto iter = voxel_map_.find(position); - if (iter != voxel_map_.end()) { voxel_map_[position]->UpdateOctoTree(p_v); } - else - { - VoxelOctoTree *octo_tree = new VoxelOctoTree(max_layer, 0, layer_init_num[0], max_points_num, planer_threshold); + if (iter != voxel_map_.end()) { + voxel_map_[position]->UpdateOctoTree(p_v); + } else { + VoxelOctoTree *octo_tree = new VoxelOctoTree( + max_layer, 0, layer_init_num[0], max_points_num, planer_threshold); voxel_map_[position] = octo_tree; voxel_map_[position]->layer_init_num_ = layer_init_num; voxel_map_[position]->quater_length_ = voxel_size / 4; @@ -639,9 +754,63 @@ void VoxelMapManager::UpdateVoxelMap(const std::vector &input_poin } } } +#endif -void VoxelMapManager::BuildResidualListOMP(std::vector &pv_list, std::vector &ptpl_list) -{ +void VoxelMapManager::UpdateVoxelMapLRU( + const std::vector &input_points) { + float voxel_size = config_setting_.max_voxel_size_; + float planer_threshold = config_setting_.planner_threshold_; + int max_layer = config_setting_.max_layer_; + int max_points_num = config_setting_.max_points_num_; + std::vector layer_init_num = config_setting_.layer_init_num_; + uint plsize = input_points.size(); + for (uint i = 0; i < plsize; i++) { + const pointWithVar p_v = input_points[i]; + float loc_xyz[3]; + for (int j = 0; j < 3; j++) { + loc_xyz[j] = p_v.point_w[j] / voxel_size; + if (loc_xyz[j] < 0) { + loc_xyz[j] -= 1.0; + } + } + VOXEL_LOCATION position((int64_t)loc_xyz[0], (int64_t)loc_xyz[1], + (int64_t)loc_xyz[2]); + auto iter = vm_map_.find(position); + if (iter != vm_map_.end()) { + vm_map_[position]->second->UpdateOctoTree(p_v); + // 更新的放至最前 + vm_data_.splice(vm_data_.begin(), vm_data_, iter->second); + iter->second = vm_data_.begin(); + } else { + VoxelOctoTree *octo_tree = new VoxelOctoTree( + max_layer, 0, layer_init_num[0], max_points_num, planer_threshold); + vm_data_.push_front({position, {octo_tree}}); + vm_map_.insert({position, vm_data_.begin()}); + + // LRU + if (vm_data_.size() >= lru_size_) { + // 删除一个尾部的数据 + vm_map_.erase(vm_data_.back().first); + delete vm_data_.back().second; + vm_data_.pop_back(); + } + + vm_map_[position]->second->quater_length_ = voxel_size / 4; + vm_map_[position]->second->voxel_center_[0] = + (0.5 + position.x) * voxel_size; + vm_map_[position]->second->voxel_center_[1] = + (0.5 + position.y) * voxel_size; + vm_map_[position]->second->voxel_center_[2] = + (0.5 + position.z) * voxel_size; + vm_map_[position]->second->layer_init_num_ = layer_init_num; + vm_map_[position]->second->UpdateOctoTree(p_v); + } + } +} + +#if 0 +void VoxelMapManager::BuildResidualListOMP( + std::vector &pv_list, std::vector &ptpl_list) { int max_layer = config_setting_.max_layer_; double voxel_size = config_setting_.max_voxel_size_; double sigma_num = config_setting_.sigma_num_; @@ -650,96 +819,194 @@ void VoxelMapManager::BuildResidualListOMP(std::vector &pv_list, s std::vector all_ptpl_list(pv_list.size()); std::vector useful_ptpl(pv_list.size()); std::vector index(pv_list.size()); - for (size_t i = 0; i < index.size(); ++i) - { + for (size_t i = 0; i < index.size(); ++i) { index[i] = i; useful_ptpl[i] = false; } - #ifdef MP_EN - omp_set_num_threads(MP_PROC_NUM); - #pragma omp parallel for - #endif - for (int i = 0; i < index.size(); i++) - { +#ifdef MP_EN + omp_set_num_threads(MP_PROC_NUM); +#pragma omp parallel for +#endif + for (int i = 0; i < index.size(); i++) { pointWithVar &pv = pv_list[i]; float loc_xyz[3]; - for (int j = 0; j < 3; j++) - { + for (int j = 0; j < 3; j++) { loc_xyz[j] = pv.point_w[j] / voxel_size; - if (loc_xyz[j] < 0) { loc_xyz[j] -= 1.0; } + if (loc_xyz[j] < 0) { + loc_xyz[j] -= 1.0; + } } - VOXEL_LOCATION position((int64_t)loc_xyz[0], (int64_t)loc_xyz[1], (int64_t)loc_xyz[2]); + VOXEL_LOCATION position((int64_t)loc_xyz[0], (int64_t)loc_xyz[1], + (int64_t)loc_xyz[2]); auto iter = voxel_map_.find(position); - if (iter != voxel_map_.end()) - { + if (iter != voxel_map_.end()) { VoxelOctoTree *current_octo = iter->second; PointToPlane single_ptpl; bool is_sucess = false; double prob = 0; - build_single_residual(pv, current_octo, 0, is_sucess, prob, single_ptpl); - if (!is_sucess) - { + BuildSingleResidual(pv, current_octo, 0, is_sucess, prob, single_ptpl); + if (!is_sucess) { VOXEL_LOCATION near_position = position; - if (loc_xyz[0] > (current_octo->voxel_center_[0] + current_octo->quater_length_)) { near_position.x = near_position.x + 1; } - else if (loc_xyz[0] < (current_octo->voxel_center_[0] - current_octo->quater_length_)) { near_position.x = near_position.x - 1; } - if (loc_xyz[1] > (current_octo->voxel_center_[1] + current_octo->quater_length_)) { near_position.y = near_position.y + 1; } - else if (loc_xyz[1] < (current_octo->voxel_center_[1] - current_octo->quater_length_)) { near_position.y = near_position.y - 1; } - if (loc_xyz[2] > (current_octo->voxel_center_[2] + current_octo->quater_length_)) { near_position.z = near_position.z + 1; } - else if (loc_xyz[2] < (current_octo->voxel_center_[2] - current_octo->quater_length_)) { near_position.z = near_position.z - 1; } + if (loc_xyz[0] > + (current_octo->voxel_center_[0] + current_octo->quater_length_)) { + near_position.x = near_position.x + 1; + } else if (loc_xyz[0] < (current_octo->voxel_center_[0] - + current_octo->quater_length_)) { + near_position.x = near_position.x - 1; + } + if (loc_xyz[1] > + (current_octo->voxel_center_[1] + current_octo->quater_length_)) { + near_position.y = near_position.y + 1; + } else if (loc_xyz[1] < (current_octo->voxel_center_[1] - + current_octo->quater_length_)) { + near_position.y = near_position.y - 1; + } + if (loc_xyz[2] > + (current_octo->voxel_center_[2] + current_octo->quater_length_)) { + near_position.z = near_position.z + 1; + } else if (loc_xyz[2] < (current_octo->voxel_center_[2] - + current_octo->quater_length_)) { + near_position.z = near_position.z - 1; + } auto iter_near = voxel_map_.find(near_position); - if (iter_near != voxel_map_.end()) { build_single_residual(pv, iter_near->second, 0, is_sucess, prob, single_ptpl); } + if (iter_near != voxel_map_.end()) { + BuildSingleResidual(pv, iter_near->second, 0, is_sucess, prob, + single_ptpl); + } } - if (is_sucess) - { + if (is_sucess) { mylock.lock(); useful_ptpl[i] = true; all_ptpl_list[i] = single_ptpl; mylock.unlock(); + } else { + mylock.lock(); + useful_ptpl[i] = false; + mylock.unlock(); } - else - { + } + } + + for (size_t i = 0; i < useful_ptpl.size(); i++) { + if (useful_ptpl[i]) { + ptpl_list.push_back(all_ptpl_list[i]); + } + } +} +#endif + +void VoxelMapManager::BuildResidualListLRU( + std::vector &pv_list, std::vector &ptpl_list) { + int max_layer = config_setting_.max_layer_; + double voxel_size = config_setting_.max_voxel_size_; + std::mutex mylock; + ptpl_list.clear(); + std::vector all_ptpl_list(pv_list.size()); + std::vector useful_ptpl(pv_list.size()); + std::vector index(pv_list.size()); + for (size_t i = 0; i < index.size(); ++i) { + index[i] = i; + useful_ptpl[i] = false; + } + for (int i = 0; i < index.size(); i++) { + pointWithVar &pv = pv_list[i]; + float loc_xyz[3]; + for (int j = 0; j < 3; j++) { + loc_xyz[j] = pv.point_w[j] / voxel_size; + if (loc_xyz[j] < 0) { + loc_xyz[j] -= 1.0; + } + } + VOXEL_LOCATION position((int64_t)loc_xyz[0], (int64_t)loc_xyz[1], + (int64_t)loc_xyz[2]); + auto iter = vm_map_.find(position); + if (iter != vm_map_.end()) { + VoxelOctoTree *current_octo = iter->second->second; + PointToPlane single_ptpl; + bool is_sucess = false; + double prob = 0; + BuildSingleResidual(pv, current_octo, 0, is_sucess, prob, single_ptpl); + if (!is_sucess) { + VOXEL_LOCATION near_position = position; + if (loc_xyz[0] > + (current_octo->voxel_center_[0] + current_octo->quater_length_)) { + near_position.x = near_position.x + 1; + } else if (loc_xyz[0] < (current_octo->voxel_center_[0] - + current_octo->quater_length_)) { + near_position.x = near_position.x - 1; + } + if (loc_xyz[1] > + (current_octo->voxel_center_[1] + current_octo->quater_length_)) { + near_position.y = near_position.y + 1; + } else if (loc_xyz[1] < (current_octo->voxel_center_[1] - + current_octo->quater_length_)) { + near_position.y = near_position.y - 1; + } + if (loc_xyz[2] > + (current_octo->voxel_center_[2] + current_octo->quater_length_)) { + near_position.z = near_position.z + 1; + } else if (loc_xyz[2] < (current_octo->voxel_center_[2] - + current_octo->quater_length_)) { + near_position.z = near_position.z - 1; + } + auto iter_near = vm_map_.find(near_position); + if (iter_near != vm_map_.end()) { + BuildSingleResidual(pv, iter_near->second->second, 0, is_sucess, prob, + single_ptpl); + } + } + if (is_sucess) { + mylock.lock(); + useful_ptpl[i] = true; + all_ptpl_list[i] = single_ptpl; + mylock.unlock(); + } else { mylock.lock(); useful_ptpl[i] = false; mylock.unlock(); } } } - for (size_t i = 0; i < useful_ptpl.size(); i++) - { - if (useful_ptpl[i]) { ptpl_list.push_back(all_ptpl_list[i]); } + for (size_t i = 0; i < useful_ptpl.size(); i++) { + if (useful_ptpl[i]) { + ptpl_list.push_back(all_ptpl_list[i]); + } } } -void VoxelMapManager::build_single_residual(pointWithVar &pv, const VoxelOctoTree *current_octo, const int current_layer, bool &is_sucess, - double &prob, PointToPlane &single_ptpl) -{ +void VoxelMapManager::BuildSingleResidual(pointWithVar &pv, + const VoxelOctoTree *current_octo, + const int current_layer, + bool &is_sucess, double &prob, + PointToPlane &single_ptpl) { int max_layer = config_setting_.max_layer_; double sigma_num = config_setting_.sigma_num_; double radius_k = 3; Eigen::Vector3d p_w = pv.point_w; - if (current_octo->plane_ptr_->is_plane_) - { + if (current_octo->plane_ptr_->is_plane_) { VoxelPlane &plane = *current_octo->plane_ptr_; Eigen::Vector3d p_world_to_center = p_w - plane.center_; - float dis_to_plane = fabs(plane.normal_(0) * p_w(0) + plane.normal_(1) * p_w(1) + plane.normal_(2) * p_w(2) + plane.d_); - float dis_to_center = (plane.center_(0) - p_w(0)) * (plane.center_(0) - p_w(0)) + (plane.center_(1) - p_w(1)) * (plane.center_(1) - p_w(1)) + - (plane.center_(2) - p_w(2)) * (plane.center_(2) - p_w(2)); + float dis_to_plane = + fabs(plane.normal_(0) * p_w(0) + plane.normal_(1) * p_w(1) + + plane.normal_(2) * p_w(2) + plane.d_); + float dis_to_center = + (plane.center_(0) - p_w(0)) * (plane.center_(0) - p_w(0)) + + (plane.center_(1) - p_w(1)) * (plane.center_(1) - p_w(1)) + + (plane.center_(2) - p_w(2)) * (plane.center_(2) - p_w(2)); float range_dis = sqrt(dis_to_center - dis_to_plane * dis_to_plane); - if (range_dis <= radius_k * plane.radius_) - { + if (range_dis <= radius_k * plane.radius_) { Eigen::Matrix J_nq; J_nq.block<1, 3>(0, 0) = p_w - plane.center_; J_nq.block<1, 3>(0, 3) = -plane.normal_; double sigma_l = J_nq * plane.plane_var_ * J_nq.transpose(); sigma_l += plane.normal_.transpose() * pv.var * plane.normal_; - if (dis_to_plane < sigma_num * sqrt(sigma_l)) - { + if (dis_to_plane < sigma_num * sqrt(sigma_l)) { is_sucess = true; - double this_prob = 1.0 / (sqrt(sigma_l)) * exp(-0.5 * dis_to_plane * dis_to_plane / sigma_l); - if (this_prob > prob) - { + double this_prob = 1.0 / (sqrt(sigma_l)) * + exp(-0.5 * dis_to_plane * dis_to_plane / sigma_l); + if (this_prob > prob) { prob = this_prob; pv.normal = plane.normal_; single_ptpl.body_cov_ = pv.body_var; @@ -750,43 +1017,37 @@ void VoxelMapManager::build_single_residual(pointWithVar &pv, const VoxelOctoTre single_ptpl.center_ = plane.center_; single_ptpl.d_ = plane.d_; single_ptpl.layer_ = current_layer; - single_ptpl.dis_to_plane_ = plane.normal_(0) * p_w(0) + plane.normal_(1) * p_w(1) + plane.normal_(2) * p_w(2) + plane.d_; + single_ptpl.dis_to_plane_ = plane.normal_(0) * p_w(0) + + plane.normal_(1) * p_w(1) + + plane.normal_(2) * p_w(2) + plane.d_; } return; - } - else - { + } else { // is_sucess = false; return; } - } - else - { + } else { // is_sucess = false; return; } - } - else - { - if (current_layer < max_layer) - { - for (size_t leafnum = 0; leafnum < 8; leafnum++) - { - if (current_octo->leaves_[leafnum] != nullptr) - { - + } else { + if (current_layer < max_layer) { + for (size_t leafnum = 0; leafnum < 8; leafnum++) { + if (current_octo->leaves_[leafnum] != nullptr) { VoxelOctoTree *leaf_octo = current_octo->leaves_[leafnum]; - build_single_residual(pv, leaf_octo, current_layer + 1, is_sucess, prob, single_ptpl); + BuildSingleResidual(pv, leaf_octo, current_layer + 1, is_sucess, prob, + single_ptpl); } } return; + } else { + return; } - else { return; } } } -void VoxelMapManager::pubVoxelMap() -{ +#if 0 +void VoxelMapManager::PubVoxelMap() { double max_trace = 0.25; double pow_num = 0.2; ros::Rate loop(500); @@ -794,49 +1055,95 @@ void VoxelMapManager::pubVoxelMap() visualization_msgs::MarkerArray voxel_plane; voxel_plane.markers.reserve(1000000); std::vector pub_plane_list; - for (auto iter = voxel_map_.begin(); iter != voxel_map_.end(); iter++) - { + for (auto iter = voxel_map_.begin(); iter != voxel_map_.end(); iter++) { GetUpdatePlane(iter->second, config_setting_.max_layer_, pub_plane_list); } - for (size_t i = 0; i < pub_plane_list.size(); i++) - { + for (size_t i = 0; i < pub_plane_list.size(); i++) { V3D plane_cov = pub_plane_list[i].plane_var_.block<3, 3>(0, 0).diagonal(); double trace = plane_cov.sum(); - if (trace >= max_trace) { trace = max_trace; } + if (trace >= max_trace) { + trace = max_trace; + } trace = trace * (1.0 / max_trace); trace = pow(trace, pow_num); uint8_t r, g, b; - mapJet(trace, 0, 1, r, g, b); + MapJet(trace, 0, 1, r, g, b); Eigen::Vector3d plane_rgb(r / 256.0, g / 256.0, b / 256.0); double alpha; - if (pub_plane_list[i].is_plane_) { alpha = use_alpha; } - else { alpha = 0; } - pubSinglePlane(voxel_plane, "plane", pub_plane_list[i], alpha, plane_rgb); + if (pub_plane_list[i].is_plane_) { + alpha = use_alpha; + } else { + alpha = 0; + } + PubSinglePlane(voxel_plane, "plane", pub_plane_list[i], alpha, plane_rgb); } voxel_map_pub_.publish(voxel_plane); loop.sleep(); } +#endif -void VoxelMapManager::GetUpdatePlane(const VoxelOctoTree *current_octo, const int pub_max_voxel_layer, std::vector &plane_list) -{ - if (current_octo->layer_ > pub_max_voxel_layer) { return; } - if (current_octo->plane_ptr_->is_update_) { plane_list.push_back(*current_octo->plane_ptr_); } - if (current_octo->layer_ < current_octo->max_layer_) - { - if (!current_octo->plane_ptr_->is_plane_) - { - for (size_t i = 0; i < 8; i++) - { - if (current_octo->leaves_[i] != nullptr) { GetUpdatePlane(current_octo->leaves_[i], pub_max_voxel_layer, plane_list); } +void VoxelMapManager::PubVoxelMapLRU() { + double max_trace = 0.25; + double pow_num = 0.2; + ros::Rate loop(500); + float use_alpha = 0.8; + visualization_msgs::MarkerArray voxel_plane; + voxel_plane.markers.reserve(1000000); + std::vector pub_plane_list; + for (auto iter = vm_map_.begin(); iter != vm_map_.end(); iter++) { + GetUpdatePlane(iter->second->second, config_setting_.max_layer_, + pub_plane_list); + } + for (size_t i = 0; i < pub_plane_list.size(); i++) { + V3D plane_cov = pub_plane_list[i].plane_var_.block<3, 3>(0, 0).diagonal(); + double trace = plane_cov.sum(); + if (trace >= max_trace) { + trace = max_trace; + } + trace = trace * (1.0 / max_trace); + trace = pow(trace, pow_num); + uint8_t r, g, b; + MapJet(trace, 0, 1, r, g, b); + Eigen::Vector3d plane_rgb(r / 256.0, g / 256.0, b / 256.0); + double alpha; + if (pub_plane_list[i].is_plane_) { + alpha = use_alpha; + } else { + alpha = 0; + } + PubSinglePlane(voxel_plane, "plane", pub_plane_list[i], alpha, plane_rgb); + } + voxel_map_pub_.publish(voxel_plane); + loop.sleep(); +} + +void VoxelMapManager::GetUpdatePlane(const VoxelOctoTree *current_octo, + const int pub_max_voxel_layer, + std::vector &plane_list) { + if (current_octo->layer_ > pub_max_voxel_layer) { + return; + } + if (current_octo->plane_ptr_->is_update_) { + plane_list.push_back(*current_octo->plane_ptr_); + } + if (current_octo->layer_ < current_octo->max_layer_) { + if (!current_octo->plane_ptr_->is_plane_) { + for (size_t i = 0; i < 8; i++) { + if (current_octo->leaves_[i] != nullptr) { + GetUpdatePlane(current_octo->leaves_[i], pub_max_voxel_layer, + plane_list); + } } } } return; } -void VoxelMapManager::pubSinglePlane(visualization_msgs::MarkerArray &plane_pub, const std::string plane_ns, const VoxelPlane &single_plane, - const float alpha, const Eigen::Vector3d rgb) -{ +void VoxelMapManager::PubSinglePlane(visualization_msgs::MarkerArray &plane_pub, + const std::string plane_ns, + const VoxelPlane &single_plane, + const float alpha, + const Eigen::Vector3d rgb) { visualization_msgs::Marker plane; plane.header.frame_id = "camera_init"; plane.header.stamp = ros::Time(); @@ -848,7 +1155,8 @@ void VoxelMapManager::pubSinglePlane(visualization_msgs::MarkerArray &plane_pub, plane.pose.position.y = single_plane.center_[1]; plane.pose.position.z = single_plane.center_[2]; geometry_msgs::Quaternion q; - CalcVectQuation(single_plane.x_normal_, single_plane.y_normal_, single_plane.normal_, q); + CalcVectQuation(single_plane.x_normal_, single_plane.y_normal_, + single_plane.normal_, q); plane.pose.orientation = q; plane.scale.x = 3 * sqrt(single_plane.max_eigen_value_); plane.scale.y = 3 * sqrt(single_plane.mid_eigen_value_); @@ -861,11 +1169,13 @@ void VoxelMapManager::pubSinglePlane(visualization_msgs::MarkerArray &plane_pub, plane_pub.markers.push_back(plane); } -void VoxelMapManager::CalcVectQuation(const Eigen::Vector3d &x_vec, const Eigen::Vector3d &y_vec, const Eigen::Vector3d &z_vec, - geometry_msgs::Quaternion &q) -{ +void VoxelMapManager::CalcVectQuation(const Eigen::Vector3d &x_vec, + const Eigen::Vector3d &y_vec, + const Eigen::Vector3d &z_vec, + geometry_msgs::Quaternion &q) { Eigen::Matrix3d rot; - rot << x_vec(0), x_vec(1), x_vec(2), y_vec(0), y_vec(1), y_vec(2), z_vec(0), z_vec(1), z_vec(2); + rot << x_vec(0), x_vec(1), x_vec(2), y_vec(0), y_vec(1), y_vec(2), z_vec(0), + z_vec(1), z_vec(2); Eigen::Matrix3d rotation = rot.transpose(); Eigen::Quaterniond eq(rotation); q.w = eq.w(); @@ -874,43 +1184,38 @@ void VoxelMapManager::CalcVectQuation(const Eigen::Vector3d &x_vec, const Eigen: q.z = eq.z(); } -void VoxelMapManager::mapJet(double v, double vmin, double vmax, uint8_t &r, uint8_t &g, uint8_t &b) -{ +void VoxelMapManager::MapJet(double v, double vmin, double vmax, uint8_t &r, + uint8_t &g, uint8_t &b) { r = 255; g = 255; b = 255; - if (v < vmin) { v = vmin; } + if (v < vmin) { + v = vmin; + } - if (v > vmax) { v = vmax; } + if (v > vmax) { + v = vmax; + } double dr, dg, db; - if (v < 0.1242) - { + if (v < 0.1242) { db = 0.504 + ((1. - 0.504) / 0.1242) * v; dg = dr = 0.; - } - else if (v < 0.3747) - { + } else if (v < 0.3747) { db = 1.; dr = 0.; dg = (v - 0.1242) * (1. / (0.3747 - 0.1242)); - } - else if (v < 0.6253) - { + } else if (v < 0.6253) { db = (0.6253 - v) * (1. / (0.6253 - 0.3747)); dg = 1.; dr = (v - 0.3747) * (1. / (0.6253 - 0.3747)); - } - else if (v < 0.8758) - { + } else if (v < 0.8758) { db = 0.; dr = 1.; dg = (0.8758 - v) * (1. / (0.8758 - 0.6253)); - } - else - { + } else { db = 0.; dg = 0.; dr = 1. - (v - 0.8758) * ((1. - 0.504) / (1. - 0.8758)); @@ -921,51 +1226,62 @@ void VoxelMapManager::mapJet(double v, double vmin, double vmax, uint8_t &r, uin b = (uint8_t)(255 * db); } -void VoxelMapManager::mapSliding() -{ - if((position_last_ - last_slide_position).norm() < config_setting_.sliding_thresh) - { - std::cout<first; - bool should_remove = loc.x > x_max || loc.x < x_min || loc.y > y_max || loc.y < y_min || loc.z > z_max || loc.z < z_min; - if (should_remove){ - // last_delete_time = omp_get_wtime(); - delete it->second; - it = voxel_map_.erase(it); - // delete_time += omp_get_wtime() - last_delete_time; - delete_voxel_cout++; - } else { - ++it; - } - } - std::cout<first; + // bool should_remove = loc.x > x_max || loc.x < x_min || loc.y > y_max || + // loc.y < y_min || loc.z > z_max || loc.z < z_min; + // if (should_remove) { + // // last_delete_time = omp_get_wtime(); + // delete it->second; + // it = voxel_map_.erase(it); + // // delete_time += omp_get_wtime() - last_delete_time; + // delete_voxel_cout++; + // } else { + // ++it; + // } + // } + // std::cout << RED << "[DEBUG]: Delete " << delete_voxel_cout << " root + // voxels" + // << RESET << "\n"; + // // std::cout< + +#include + +#include "cartesian.hpp" +#include "common.hpp" +#include "rxso2.hpp" +#include "rxso3.hpp" +#include "se2.hpp" +#include "se3.hpp" +#include "sim2.hpp" +#include "sim3.hpp" +#include "so2.hpp" +#include "so3.hpp" + +namespace Sophus { + +/// Calculates mean iteratively. +/// +/// Returns ``nullopt`` if it does not converge. +/// +template +std::optional iterativeMean( + SequenceContainer const& foo_Ts_bar, int max_num_iterations) { + size_t N = foo_Ts_bar.size(); + SOPHUS_ENSURE(N >= 1, "N must be >= 1."); + + using Group = typename SequenceContainer::value_type; + using Scalar = typename Group::Scalar; + using Tangent = typename Group::Tangent; + + // This implements the algorithm in the beginning of Sec. 4.2 in + // ftp://ftp-sop.inria.fr/epidaure/Publications/Arsigny/arsigny_rr_biinvariant_average.pdf. + Group foo_T_average = foo_Ts_bar.front(); + Scalar w = Scalar(1. / N); + for (int i = 0; i < max_num_iterations; ++i) { + Tangent average; + setToZero(average); + for (Group const& foo_T_bar : foo_Ts_bar) { + average += w * (foo_T_average.inverse() * foo_T_bar).log(); + } + Group foo_T_newaverage = foo_T_average * Group::exp(average); + if (squaredNorm( + (foo_T_newaverage.inverse() * foo_T_average).log()) < + Constants::epsilon()) { + return foo_T_newaverage; + } + + foo_T_average = foo_T_newaverage; + } + // LCOV_EXCL_START + return std::nullopt; + // LCOV_EXCL_STOP +} + +// Mean implementation for Cartesian. +template +std::enable_if_t >::value, + std::optional > +average(SequenceContainer const& foo_Ts_bar) { + size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar)); + SOPHUS_ENSURE(N >= 1, "N must be >= 1."); + + Sophus::Vector average; + average.setZero(); + for (Cartesian const& foo_T_bar : foo_Ts_bar) { + average += foo_T_bar.params(); + } + return Cartesian(average / Scalar(N)); +} + +// Mean implementation for SO(2). +template +std::enable_if_t< + std::is_same >::value, + std::optional > +average(SequenceContainer const& foo_Ts_bar) { + // This implements rotational part of Proposition 12 from Sec. 6.2 of + // ftp://ftp-sop.inria.fr/epidaure/Publications/Arsigny/arsigny_rr_biinvariant_average.pdf. + size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar)); + SOPHUS_ENSURE(N >= 1, "N must be >= 1."); + SO2 foo_T_average = foo_Ts_bar.front(); + Scalar w = Scalar(1. / N); + + Scalar average(0); + for (SO2 const& foo_T_bar : foo_Ts_bar) { + average += w * (foo_T_average.inverse() * foo_T_bar).log(); + } + return foo_T_average * SO2::exp(average); +} + +// Mean implementation for RxSO(2). +template +std::enable_if_t< + std::is_same >::value, + std::optional > +average(SequenceContainer const& foo_Ts_bar) { + size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar)); + SOPHUS_ENSURE(N >= 1, "N must be >= 1."); + RxSO2 foo_T_average = foo_Ts_bar.front(); + Scalar w = Scalar(1. / N); + + Vector2 average(Scalar(0), Scalar(0)); + for (RxSO2 const& foo_T_bar : foo_Ts_bar) { + average += w * (foo_T_average.inverse() * foo_T_bar).log(); + } + return foo_T_average * RxSO2::exp(average); +} + +namespace details { +template +void getQuaternion(T const&); + +template +Eigen::Quaternion getUnitQuaternion(SO3 const& R) { + return R.unit_quaternion(); +} + +template +Eigen::Quaternion getUnitQuaternion(RxSO3 const& sR) { + return sR.so3().unit_quaternion(); +} + +template +Eigen::Quaternion averageUnitQuaternion( + SequenceContainer const& foo_Ts_bar) { + // This: http://stackoverflow.com/a/27410865/1221742 + size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar)); + SOPHUS_ENSURE(N >= 1, "N must be >= 1."); + Eigen::Matrix Q(4, N); + int i = 0; + Scalar w = Scalar(1. / N); + for (auto const& foo_T_bar : foo_Ts_bar) { + Q.col(i) = w * details::getUnitQuaternion(foo_T_bar).coeffs(); + ++i; + } + + Eigen::Matrix QQt = Q * Q.transpose(); + // TODO: Figure out why we can't use SelfAdjointEigenSolver here. + Eigen::EigenSolver > es(QQt); + + std::complex max_eigenvalue = es.eigenvalues()[0]; + Eigen::Matrix, 4, 1> max_eigenvector = + es.eigenvectors().col(0); + + for (int i = 1; i < 4; i++) { + if (std::norm(es.eigenvalues()[i]) > std::norm(max_eigenvalue)) { + max_eigenvalue = es.eigenvalues()[i]; + max_eigenvector = es.eigenvectors().col(i); + } + } + Eigen::Quaternion quat; + quat.coeffs() << // + max_eigenvector[0].real(), // + max_eigenvector[1].real(), // + max_eigenvector[2].real(), // + max_eigenvector[3].real(); + return quat; +} +} // namespace details + +// Mean implementation for SO(3). +// +// TODO: Detect degenerated cases and return nullopt. +template +std::enable_if_t< + std::is_same >::value, + std::optional > +average(SequenceContainer const& foo_Ts_bar) { + return SO3(details::averageUnitQuaternion(foo_Ts_bar)); +} + +// Mean implementation for R x SO(3). +template +std::enable_if_t< + std::is_same >::value, + std::optional > +average(SequenceContainer const& foo_Ts_bar) { + size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar)); + + SOPHUS_ENSURE(N >= 1, "N must be >= 1."); + Scalar scale_sum = Scalar(0); + using std::exp; + using std::log; + for (RxSO3 const& foo_T_bar : foo_Ts_bar) { + scale_sum += log(foo_T_bar.scale()); + } + return RxSO3(exp(scale_sum / Scalar(N)), + SO3(details::averageUnitQuaternion(foo_Ts_bar))); +} + +template +std::enable_if_t< + std::is_same >::value, + std::optional > +average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) { + // TODO: Implement Proposition 12 from Sec. 6.2 of + // ftp://ftp-sop.inria.fr/epidaure/Publications/Arsigny/arsigny_rr_biinvariant_average.pdf. + return iterativeMean(foo_Ts_bar, max_num_iterations); +} + +template +std::enable_if_t< + std::is_same >::value, + std::optional > +average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) { + return iterativeMean(foo_Ts_bar, max_num_iterations); +} + +template +std::enable_if_t< + std::is_same >::value, + std::optional > +average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) { + return iterativeMean(foo_Ts_bar, max_num_iterations); +} + +template +std::enable_if_t< + std::is_same >::value, + std::optional > +average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) { + return iterativeMean(foo_Ts_bar, max_num_iterations); +} + +} // namespace Sophus diff --git a/vikit_common/include/sophus/cartesian.hpp b/vikit_common/include/sophus/cartesian.hpp new file mode 100644 index 00000000..ee3e210a --- /dev/null +++ b/vikit_common/include/sophus/cartesian.hpp @@ -0,0 +1,524 @@ +/// @file +/// Cartesian - Euclidean vector space as Lie group + +#pragma once +#include + +namespace Sophus { +template +class Cartesian; + +template +using Cartesian2 = Cartesian; + +template +using Cartesian3 = Cartesian; + +using Cartesian2d = Cartesian2; +using Cartesian3d = Cartesian3; + +} // namespace Sophus + +namespace Eigen { +namespace internal { + +template +struct traits> { + using Scalar = Scalar_; + using ParamsType = Sophus::Vector; +}; + +template +struct traits, Options>> + : traits> { + using Scalar = Scalar_; + using ParamsType = Map, Options>; +}; + +template +struct traits const, Options>> + : traits const> { + using Scalar = Scalar_; + using ParamsType = Map const, Options>; +}; +} // namespace internal +} // namespace Eigen + +namespace Sophus { + +/// Cartesian base type - implements Cartesian class but is storage agnostic. +/// +/// Euclidean vector space as Lie group. +/// +/// Lie groups can be seen as a generalization over the Euclidean vector +/// space R^M. Here a N-dimensional vector ``p`` is represented as a +// (M+1) x (M+1) homogeneous matrix: +/// +/// | I p | +/// | o 1 | +/// +/// On the other hand, Cartesian(M) can be seen as a special case of SE(M) +/// with identity rotation, and hence represents pure translation. +/// +/// The purpose of this class is two-fold: +/// - for educational purpose, to highlight how Lie groups generalize over +/// Euclidean vector spaces. +/// - to be used in templated/generic algorithms (such as Sophus::Spline) +/// which are implemented against the Lie group interface. +/// +/// Obviously, Cartesian(M) can just be represented as a M-tuple. +/// +/// Cartesian is not compact, but a commutative group. For vector additions it +/// holds `a+b = b+a`. +/// +/// See Cartesian class for more details below. +/// +template +class CartesianBase { + public: + using Scalar = typename Eigen::internal::traits::Scalar; + using ParamsType = typename Eigen::internal::traits::ParamsType; + /// Degrees of freedom of manifold, equals to number of Cartesian coordinates. + static int constexpr DoF = M; + /// Number of internal parameters used, also M. + static int constexpr num_parameters = M; + /// Group transformations are (M+1)x(M+1) matrices. + static int constexpr N = M + 1; + static int constexpr Dim = M; + + using Transformation = Sophus::Matrix; + using Point = Sophus::Vector; + using HomogeneousPoint = Sophus::Vector; + using Line = ParametrizedLine; + using Hyperplane = Eigen::Hyperplane; + using Tangent = Sophus::Vector; + using Adjoint = Matrix; + + /// For binary operations the return type is determined with the + /// ScalarBinaryOpTraits feature of Eigen. This allows mixing concrete and Map + /// types, as well as other compatible scalar types such as Ceres::Jet and + /// double scalars with Cartesian operations. + template + using ReturnScalar = typename Eigen::ScalarBinaryOpTraits< + Scalar, typename OtherDerived::Scalar>::ReturnType; + + template + using CartesianSum = Cartesian, M>; + + template + using PointProduct = Sophus::Vector, M>; + + template + using HomogeneousPointProduct = + Sophus::Vector, N>; + + /// Adjoint transformation + /// + /// Always identity of commutative groups. + SOPHUS_FUNC Adjoint Adj() const { return Adjoint::Identity(); } + + /// Returns copy of instance casted to NewScalarType. + /// + template + SOPHUS_FUNC Cartesian cast() const { + return Cartesian(params().template cast()); + } + + /// Returns derivative of this * exp(x) wrt x at x=0. + /// + SOPHUS_FUNC Matrix Dx_this_mul_exp_x_at_0() + const { + Sophus::Matrix m; + m.setIdentity(); + return m; + } + + /// Returns derivative of log(this^{-1} * x) by x at x=this. + /// + SOPHUS_FUNC Matrix Dx_log_this_inv_by_x_at_this() + const { + Matrix m; + m.setIdentity(); + return m; + } + + /// Returns group inverse. + /// + /// The additive inverse. + /// + SOPHUS_FUNC Cartesian inverse() const { + return Cartesian(-params()); + } + + /// Logarithmic map + /// + /// For Euclidean vector space, just the identity. Or to be more precise + /// it just extracts the significant M-vector from the NxN matrix. + /// + SOPHUS_FUNC Tangent log() const { return params(); } + + /// Returns 4x4 matrix representation of the instance. + /// + /// It has the following form: + /// + /// | I p | + /// | o 1 | + /// + SOPHUS_FUNC Transformation matrix() const { + Sophus::Matrix matrix; + matrix.setIdentity(); + matrix.col(M).template head() = params(); + return matrix; + } + + /// Group multiplication, are vector additions. + /// + template + SOPHUS_FUNC CartesianBase& operator=( + CartesianBase const& other) { + params() = other.params(); + return *this; + } + + /// Group multiplication, are vector additions. + /// + template + SOPHUS_FUNC CartesianSum operator*( + CartesianBase const& other) const { + return CartesianSum(params() + other.params()); + } + + /// Group action on points, again just vector addition. + /// + template ::value>> + SOPHUS_FUNC PointProduct operator*( + Eigen::MatrixBase const& p) const { + return PointProduct(params() + p); + } + + /// Group action on homogeneous points. See above for more details. + /// + template ::value>> + SOPHUS_FUNC HomogeneousPointProduct operator*( + Eigen::MatrixBase const& p) const { + const auto rp = *this * p.template head(); + HomogeneousPointProduct r; + r << rp, p(M); + return r; + } + + /// Group action on lines. + /// + SOPHUS_FUNC Line operator*(Line const& l) const { + return Line((*this) * l.origin(), l.direction()); + } + + /// Group action on planes. + /// + SOPHUS_FUNC Hyperplane operator*(Hyperplane const& p) const { + return Hyperplane(p.normal(), p.offset() - params().dot(p.normal())); + } + + /// In-place group multiplication. This method is only valid if the return + /// type of the multiplication is compatible with this Cartesian's Scalar + /// type. + /// + template >::value>> + SOPHUS_FUNC CartesianBase& operator*=( + CartesianBase const& other) { + *static_cast(this) = *this * other; + return *this; + } + + /// Mutator of params vector. + /// + SOPHUS_FUNC ParamsType& params() { + return static_cast(this)->params(); + } + + /// Accessor of params vector + /// + SOPHUS_FUNC ParamsType const& params() const { + return static_cast(this)->params(); + } +}; + +/// Cartesian using default storage; derived from CartesianBase. +template +class Cartesian : public CartesianBase, M> { + using Base = CartesianBase, M>; + + public: + static int constexpr DoF = Base::DoF; + static int constexpr num_parameters = Base::num_parameters; + static int constexpr N = Base::N; + static int constexpr Dim = Base::Dim; + + using Scalar = Scalar_; + using Transformation = typename Base::Transformation; + using Point = typename Base::Point; + using HomogeneousPoint = typename Base::HomogeneousPoint; + using Tangent = typename Base::Tangent; + using ParamsMember = Sophus::Vector; + + using Base::operator=; + + /// Define copy-assignment operator explicitly. The definition of + /// implicit copy assignment operator is deprecated in presence of a + /// user-declared copy constructor (-Wdeprecated-copy in clang >= 13). + SOPHUS_FUNC Cartesian& operator=(Cartesian const& other) = default; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /// Default constructor initializes to zero vector. + /// + SOPHUS_FUNC Cartesian() { params_.setZero(); } + + /// Copy constructor + /// + SOPHUS_FUNC Cartesian(Cartesian const& other) = default; + + /// Copy-like constructor from OtherDerived. + /// + template + SOPHUS_FUNC Cartesian(CartesianBase const& other) + : params_(other.params()) { + static_assert(std::is_same::value, + "must be same Scalar type"); + } + + /// Accepts either M-vector or (M+1)x(M+1) matrices. + /// + template + explicit SOPHUS_FUNC Cartesian(Eigen::MatrixBase const& m) { + static_assert( + std::is_same::Scalar, Scalar>::value, ""); + if (m.rows() == DoF && m.cols() == 1) { + // trick so this compiles + params_ = m.template block(0, 0); + } else if (m.rows() == N && m.cols() == N) { + params_ = m.template block(0, M); + } else { + SOPHUS_ENSURE(false, "{} {}", m.rows(), m.cols()); + } + } + + /// This provides unsafe read/write access to internal data. + /// + SOPHUS_FUNC Scalar* data() { return params_.data(); } + + /// Const version of data() above. + /// + SOPHUS_FUNC Scalar const* data() const { return params_.data(); } + + /// Returns derivative of exp(x) wrt. x. + /// + SOPHUS_FUNC static Sophus::Matrix + Dx_exp_x_at_0() { + Sophus::Matrix m; + m.setIdentity(); + return m; + } + + /// Returns derivative of exp(x) wrt. x_i at x=0. + /// + SOPHUS_FUNC static Sophus::Matrix Dx_exp_x( + Tangent const&) { + return Dx_exp_x_at_0(); + } + + /// Returns derivative of exp(x) * p wrt. x_i at x=0. + /// + SOPHUS_FUNC static Sophus::Matrix Dx_exp_x_times_point_at_0( + Point const&) { + Sophus::Matrix J; + J.setIdentity(); + return J; + } + + /// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``. + /// + SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) { + return generator(i); + } + + /// Mutator of params vector + /// + SOPHUS_FUNC ParamsMember& params() { return params_; } + + /// Accessor of params vector + /// + SOPHUS_FUNC ParamsMember const& params() const { return params_; } + + /// Returns the ith infinitesimal generators of Cartesian(M). + /// + /// The infinitesimal generators for e.g. the 3-dimensional case: + /// + /// ``` + /// | 0 0 0 1 | + /// G_0 = | 0 0 0 0 | + /// | 0 0 0 0 | + /// | 0 0 0 0 | + /// + /// | 0 0 0 0 | + /// G_1 = | 0 0 0 1 | + /// | 0 0 0 0 | + /// | 0 0 0 0 | + /// + /// | 0 0 0 0 | + /// G_2 = | 0 0 0 0 | + /// | 0 0 0 1 | + /// | 0 0 0 0 | + /// ``` + /// + /// Precondition: ``i`` must be in [0, M-1]. + /// + SOPHUS_FUNC static Transformation generator(int i) { + SOPHUS_ENSURE(i >= 0 && i <= M, "i should be in range [0,M-1]."); + Tangent e; + e.setZero(); + e[i] = Scalar(1); + return hat(e); + } + + /// Group exponential + /// + /// For Euclidean vector space, just the identity. Or to be more precise + /// it just constructs the (M+1xM+1) homogeneous matrix representation + // from the M-vector. + /// + SOPHUS_FUNC static Cartesian exp(Tangent const& a) { + return Cartesian(a); + } + + /// hat-operator + /// + /// Formally, the hat()-operator of Cartesian(M) is defined as + /// + /// ``hat(.): R^M -> R^{M+1xM+1}, hat(a) = sum_i a_i * G_i`` + /// (for i=0,...,M-1) + /// + /// with ``G_i`` being the ith infinitesimal generator of Cartesian(M). + /// + /// The corresponding inverse is the vee()-operator, see below. + /// + SOPHUS_FUNC static Transformation hat(Tangent const& a) { + Transformation Omega; + Omega.setZero(); + Omega.col(M).template head() = a.template head(); + return Omega; + } + + /// Lie bracket + /// + /// Always 0 for commutative groups. + SOPHUS_FUNC static Tangent lieBracket(Tangent const&, Tangent const&) { + return Tangent::Zero(); + } + + /// Draws uniform samples in the range [-1, 1] per coordinates. + /// + template + static Cartesian sampleUniform(UniformRandomBitGenerator& generator) { + std::uniform_real_distribution uniform(Scalar(-1), Scalar(1)); + Vector v; + for (int i = 0; i < M; ++i) { + v[i] = uniform(generator); + } + return Cartesian(v); + } + + /// vee-operator + /// + /// This is the inverse of the hat()-operator, see above. + /// + SOPHUS_FUNC static Tangent vee(Transformation const& m) { + return m.col(M).template head(); + } + + protected: + ParamsMember params_; +}; + +} // namespace Sophus + +namespace Eigen { + +/// Specialization of Eigen::Map for ``Cartesian``; derived from +/// CartesianBase. +/// +/// Allows us to wrap Cartesian objects around POD array. +template +class Map, Options> + : public Sophus::CartesianBase, Options>, + M> { + public: + using Base = + Sophus::CartesianBase, Options>, M>; + using Scalar = Scalar_; + using Transformation = typename Base::Transformation; + using Point = typename Base::Point; + using HomogeneousPoint = typename Base::HomogeneousPoint; + using Tangent = typename Base::Tangent; + + using Base::operator=; + using Base::operator*=; + using Base::operator*; + + SOPHUS_FUNC explicit Map(Scalar* coeffs) : params_(coeffs) {} + + /// Mutator of params vector + /// + SOPHUS_FUNC Map>& params() { + return params_; + } + + /// Accessor of params vector + /// + SOPHUS_FUNC Map> const& params() const { + return params_; + } + + protected: + Map, Options> params_; +}; + +/// Specialization of Eigen::Map for ``Cartesian const``; derived from +/// CartesianBase. +/// +/// Allows us to wrap Cartesian objects around POD array. +template +class Map const, Options> + : public Sophus::CartesianBase< + Map const, Options>, M> { + public: + using Base = + Sophus::CartesianBase const, Options>, + M>; + using Scalar = Scalar_; + using Transformation = typename Base::Transformation; + using Point = typename Base::Point; + using HomogeneousPoint = typename Base::HomogeneousPoint; + using Tangent = typename Base::Tangent; + + using Base::operator*; + + SOPHUS_FUNC Map(Scalar const* coeffs) : params_(coeffs) {} + + /// Accessor of params vector + /// + SOPHUS_FUNC Map const, Options> const& params() + const { + return params_; + } + + protected: + Map const, Options> const params_; +}; +} // namespace Eigen diff --git a/vikit_common/include/sophus/ceres_manifold.hpp b/vikit_common/include/sophus/ceres_manifold.hpp new file mode 100644 index 00000000..3e363ded --- /dev/null +++ b/vikit_common/include/sophus/ceres_manifold.hpp @@ -0,0 +1,68 @@ +#pragma once + +#include +#include + +namespace Sophus { + +/// Templated local parameterization for LieGroup [with implemented +/// LieGroup::Dx_this_mul_exp_x_at_0() ] +template