diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 00000000..4c8c8e25 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,71 @@ +{ + "files.associations": { + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "any": "cpp", + "array": "cpp", + "atomic": "cpp", + "bit": "cpp", + "*.tcc": "cpp", + "bitset": "cpp", + "chrono": "cpp", + "cinttypes": "cpp", + "compare": "cpp", + "complex": "cpp", + "concepts": "cpp", + "condition_variable": "cpp", + "cstdint": "cpp", + "deque": "cpp", + "list": "cpp", + "map": "cpp", + "set": "cpp", + "unordered_map": "cpp", + "unordered_set": "cpp", + "vector": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "functional": "cpp", + "iterator": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "optional": "cpp", + "random": "cpp", + "ratio": "cpp", + "string": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "fstream": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "mutex": "cpp", + "new": "cpp", + "ostream": "cpp", + "ranges": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "stop_token": "cpp", + "streambuf": "cpp", + "thread": "cpp", + "typeindex": "cpp", + "typeinfo": "cpp", + "variant": "cpp", + "core": "cpp" + } +} \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index e4dd277c..9e9a2bdf 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -86,7 +86,7 @@ find_package(catkin REQUIRED COMPONENTS std_msgs pcl_ros tf - livox_ros_driver + livox_ros_driver2 message_generation eigen_conversions vikit_common diff --git a/README.md b/README.md index eac5d4e6..25bf7d11 100644 --- a/README.md +++ b/README.md @@ -74,9 +74,9 @@ cd catkin_ws/src git clone https://github.com/xuankuzcr/rpg_vikit.git ``` -### 2.5 **livox_ros_driver** +### 2.5 **livox_ros_driver2** -Follow [livox_ros_driver Installation](https://github.com/Livox-SDK/livox_ros_driver). +Follow [livox_ros_driver2 Installation](https://github.com/Livox-SDK/livox_ros_driver2). ## 3. Build diff --git a/config/avia.yaml b/config/avia.yaml index 04c24bab..f5b857fc 100755 --- a/config/avia.yaml +++ b/config/avia.yaml @@ -56,7 +56,7 @@ lio: max_points_num: 50 layer_init_num: [5, 5, 5, 5, 5] -local_map: +local_map: map_sliding_en: false half_map_size: 100 sliding_thresh: 8 @@ -67,7 +67,7 @@ uav: publish: dense_map_en: true - pub_effect_point_en: false + pub_effect_point_en: false pub_plane_en: false pub_scan_num: 1 blind_rgb_points: 0.0 @@ -80,6 +80,6 @@ pcd_save: pcd_save_en: false colmap_output_en: false # need to set interval = -1 filter_size_pcd: 0.15 - interval: -1 + interval: -1 # how many LiDAR frames saved in each pcd file; # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. diff --git a/config/camera_pinhole_mid360.yaml b/config/camera_pinhole_mid360.yaml new file mode 100644 index 00000000..bc78cf9c --- /dev/null +++ b/config/camera_pinhole_mid360.yaml @@ -0,0 +1,12 @@ +cam_model: EquidistantCamera +cam_width: 1920 +cam_height: 1080 +scale: 0.5 +cam_fx: 542.02 +cam_fy: 541.678 +cam_cx: 937.291 +cam_cy: 520.273 +k1: -0.0466268 +k2: -0.00140065 +k3: -0.00140703 +k4: -1.08674e-05 diff --git a/config/mid360.yaml b/config/mid360.yaml new file mode 100644 index 00000000..d1a3f180 --- /dev/null +++ b/config/mid360.yaml @@ -0,0 +1,86 @@ +common: + img_topic: "/camera/right/image" + lid_topic: "/livox/lidar" + imu_topic: "/livox/imu" + img_en: 1 + lidar_en: 1 + ros_driver_bug_fix: false + +extrin_calib: + extrinsic_T: [0, 0, 0] + # R_I^L + extrinsic_R: [1, 0, 0, 0, 1, 0, 0, 0, 1] + # R_C^L + Rcl: [-0.78682, -0.41620, -0.45573, + 0.50295, -0.00442, -0.86430, + 0.35771, -0.90926, 0.21281] + Pcl: [-0.09886,0.04385,0.02253] + +time_offset: + imu_time_offset: 0.0 + img_time_offset: 0.1 + exposure_time_init: 0.0 + +preprocess: + point_filter_num: 1 + filter_size_surf: 0.1 + lidar_type: 1 # Livox Avia LiDAR + scan_line: 4 + blind: 0.5 + +vio: + max_iterations: 5 + outlier_threshold: 1000 # 78 100 156 #100 200 500 700 infinite + img_point_cov: 100 # 100 1000 + patch_size: 8 + patch_pyrimid_level: 4 + normal_en: true + raycast_en: false + inverse_composition_en: false + exposure_estimate_en: true + inv_expo_cov: 0.1 + +imu: + imu_en: true + imu_int_frame: 30 + acc_cov: 0.8 # 0.2 + gyr_cov: 0.5 # 0.5 + b_acc_cov: 0.001 # 0.1 + b_gyr_cov: 0.001 # 0.1 + +lio: + max_iterations: 5 + dept_err: 0.02 + beam_err: 0.15 + min_eigen_value: 0.005 # 0.005 + voxel_size: 0.5 + max_layer: 2 + max_points_num: 50 + layer_init_num: [5, 5, 5, 5, 5] + +local_map: + map_sliding_en: false + half_map_size: 100 + sliding_thresh: 8 + +uav: + imu_rate_odom: false + gravity_align_en: false + +publish: + dense_map_en: true + pub_effect_point_en: false + pub_plane_en: false + pub_scan_num: 1 + blind_rgb_points: 0.0 + +evo: + seq_name: "CBD_Building_01" + pose_output_en: false + +pcd_save: + pcd_save_en: false + colmap_output_en: false # need to set interval = -1 + filter_size_pcd: 0.15 + interval: -1 + diff --git a/include/LIVMapper.h b/include/LIVMapper.h index 6b2ec494..dd5bf14d 100644 --- a/include/LIVMapper.h +++ b/include/LIVMapper.h @@ -20,7 +20,20 @@ which is included as part of this source code package. #include #include #include - +#include +#include +struct NerfData { + PointCloudXYZI::Ptr cloud; + Eigen::Isometry3d pose; + cv::Mat image; + NerfData() + { + cloud.reset(new PointCloudXYZI()); + pose = Eigen::Isometry3d::Identity(); + } + NerfData(PointCloudXYZI::Ptr c, const Eigen::Isometry3d& p, cv::Mat img) + : cloud(c), pose(p),image(img) {} +}; class LIVMapper { public: @@ -43,10 +56,10 @@ class LIVMapper 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 SaveNerfData(); 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 livox_pcl_cbk(const livox_ros_driver2::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); @@ -182,5 +195,18 @@ class LIVMapper double aver_time_icp = 0; double aver_time_map_inre = 0; bool colmap_output_en = false; + + std::thread* save_nerf_thread_; + mutable std::shared_mutex data_mutex_; + std::vector AllCloudAndPose_; + Eigen::Isometry3d T_c_b_ = Eigen::Isometry3d::Identity(); + Eigen::Matrix3d Rcl_; + Eigen::Vector3d Pcl_; + ofstream fout_nerf_color_, fout_nerf_depth_; + int cam_width_,cam_height_; + double scale_,cam_fx_,cam_fy_,cam_cx_,cam_cy_,k1_,k2_,k3_,k4_; + std::string cam_model_; + int img_cnt_ = 1; + cv::Mat img_cur_data_; }; #endif \ No newline at end of file diff --git a/include/preprocess.h b/include/preprocess.h index 649ed838..fd96fd5e 100755 --- a/include/preprocess.h +++ b/include/preprocess.h @@ -14,7 +14,7 @@ which is included as part of this source code package. #define PREPROCESS_H_ #include "common_lib.h" -#include +#include #include using namespace std; @@ -138,7 +138,7 @@ class Preprocess Preprocess(); ~Preprocess(); - void process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out); + void process(const livox_ros_driver2::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); @@ -153,7 +153,7 @@ class Preprocess ros::Publisher pub_full, pub_surf, pub_corn; private: - void avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg); + void avia_handler(const livox_ros_driver2::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); diff --git a/launch/mapping_mid360.launch b/launch/mapping_mid360.launch new file mode 100644 index 00000000..99618b0e --- /dev/null +++ b/launch/mapping_mid360.launch @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + +launch-prefix="gdb -ex run --args" +launch-prefix="valgrind --leak-check=full" + diff --git a/package.xml b/package.xml index af3a353c..2ebef9d9 100755 --- a/package.xml +++ b/package.xml @@ -25,7 +25,7 @@ sensor_msgs tf pcl_ros - livox_ros_driver + livox_ros_driver2 message_generation vikit_common vikit_ros @@ -39,7 +39,7 @@ std_msgs tf pcl_ros - livox_ros_driver + livox_ros_driver2 message_runtime vikit_common vikit_ros diff --git a/src/LIVMapper.cpp b/src/LIVMapper.cpp index 68d4f4eb..8ce678d2 100755 --- a/src/LIVMapper.cpp +++ b/src/LIVMapper.cpp @@ -44,10 +44,78 @@ LIVMapper::LIVMapper(ros::NodeHandle &nh) initializeComponents(); path.header.stamp = ros::Time::now(); path.header.frame_id = "camera_init"; + save_nerf_thread_ = new std::thread(&LIVMapper::SaveNerfData, this); } -LIVMapper::~LIVMapper() {} +LIVMapper::~LIVMapper() +{ + ros::shutdown(); + save_nerf_thread_->join(); + delete save_nerf_thread_; +} +void LIVMapper::SaveNerfData() +{ + ros::Rate rate(20); + while (ros::ok()) + { + rate.sleep(); + NerfData data_cur; + { + std::unique_lock lock(data_mutex_); + if (AllCloudAndPose_.empty()) + continue; + data_cur = AllCloudAndPose_.front(); + AllCloudAndPose_.erase(AllCloudAndPose_.begin()); + } + cv::resize(data_cur.image, data_cur.image, cv::Size(), scale_, scale_, cv::INTER_LINEAR); + std::ostringstream ss; + ss << std::setw(5) << std::setfill('0') << img_cnt_; + std::string cnt_str = ss.str(); + std::string image_path = std::string(ROOT_DIR) + "Log/Colmap/images/" + cnt_str + ".png"; + + cv::Mat cvK = (cv::Mat_(3, 3) << cam_fx_ * scale_, 0.0, cam_cx_ * scale_, 0.0, cam_fy_ * scale_, + cam_cy_ * scale_, 0.0, 0.0, 1.0); + cv::Mat cvD = (cv::Mat_(1, 4) << k1_, k2_, k3_, k4_); + cv::Mat undist_map1_(cam_width_ * scale_, cam_height_ * scale_, CV_16SC2); + cv::Mat undist_map2_(cam_width_ * scale_, cam_height_ * scale_, CV_16SC2); + if(cam_model_ == "Pinhole") + { + cv::initUndistortRectifyMap(cvK, cvD, cv::Mat_::eye(3,3), cvK, + cv::Size(cam_width_ * scale_, cam_height_ * scale_), CV_16SC2, undist_map1_, undist_map2_); + } + else if(cam_model_ == "EquidistantCamera") + { + cv::fisheye::initUndistortRectifyMap(cvK, cvD, cv::Mat_::eye(3, 3), cvK, + cv::Size(cam_width_ * scale_, cam_height_ * scale_), CV_16SC2, undist_map1_, + undist_map2_); + } + cv::Mat rectified; + cv::remap(data_cur.image, rectified, undist_map1_, undist_map2_, cv::INTER_LINEAR); + cv::imwrite(image_path, rectified); + // T_w^c = T_w^b * T_b^c + Eigen::Isometry3d T_w_c = data_cur.pose * T_c_b_.inverse(); + for (int i = 0; i < 4; i++) + { + for (int j = 0; j < 4; j++) + { + fout_nerf_color_ << T_w_c.matrix()(i, j) << " "; + } + fout_nerf_color_ << "\n"; + } + for (int i = 0; i < 4; i++) + { + for (int j = 0; j < 4; j++) + { + fout_nerf_depth_ << data_cur.pose.matrix()(i, j) << " "; + } + fout_nerf_depth_ << "\n"; + } + std::string filename = std::string(ROOT_DIR) + "Log/Colmap/depths/" + cnt_str + ".ply"; + pcl::io::savePLYFile(filename, *data_cur.cloud); + img_cnt_++; + } +} void LIVMapper::readParameters(ros::NodeHandle &nh) { nh.param("common/lid_topic", lid_topic, "/livox/lidar"); @@ -109,6 +177,22 @@ void LIVMapper::readParameters(ros::NodeHandle &nh) nh.param("publish/dense_map_en", dense_map_en, false); p_pre->blind_sqr = p_pre->blind * p_pre->blind; + nh.param("laserMapping/cam_model", cam_model_, "EquidistantCamera"); + nh.param("laserMapping/cam_width", cam_width_, 1920); + nh.param("laserMapping/cam_height", cam_height_, 1080); + nh.param("laserMapping/scale", scale_, 0.5); + nh.param("laserMapping/cam_fx", cam_fx_, 0.5); + nh.param("laserMapping/cam_fy", cam_fy_, 0.5); + nh.param("laserMapping/cam_cx", cam_cx_, 0.5); + nh.param("laserMapping/cam_cy", cam_cy_, 0.5); + nh.param("laserMapping/k1", k1_, 0.5); + nh.param("laserMapping/k2", k2_, 0.5); + nh.param("laserMapping/k3", k3_, 0.5); + nh.param("laserMapping/k4", k4_, 0.5); + Rcl_ << MAT_FROM_ARRAY(cameraextrinR); + Pcl_ << VEC_FROM_ARRAY(cameraextrinT); + T_c_b_.linear() = Rcl_; + T_c_b_.translation() = Pcl_; } void LIVMapper::initializeComponents() @@ -159,28 +243,29 @@ void LIVMapper::initializeComponents() void LIVMapper::initializeFiles() { - if (pcd_save_en && colmap_output_en) - { - const std::string folderPath = std::string(ROOT_DIR) + "/scripts/colmap_output.sh"; - - std::string chmodCommand = "chmod +x " + folderPath; - - int chmodRet = system(chmodCommand.c_str()); - if (chmodRet != 0) { - std::cerr << "Failed to set execute permissions for the script." << std::endl; - return; - } - int executionRet = system(folderPath.c_str()); - if (executionRet != 0) { - std::cerr << "Failed to execute the script." << std::endl; - return; - } + const std::string folderPath = std::string(ROOT_DIR) + "/scripts/colmap_output.sh"; + + std::string chmodCommand = "chmod +x " + folderPath; + + int chmodRet = system(chmodCommand.c_str()); + if (chmodRet != 0) { + std::cerr << "Failed to set execute permissions for the script." << std::endl; + return; + } + + int executionRet = system(folderPath.c_str()); + if (executionRet != 0) { + std::cerr << "Failed to execute the script." << std::endl; + return; } + if(colmap_output_en) fout_points.open(std::string(ROOT_DIR) + "Log/Colmap/sparse/0/points3D.txt", std::ios::out); if(pcd_save_interval > 0) fout_pcd_pos.open(std::string(ROOT_DIR) + "Log/PCD/scans_pos.json", std::ios::out); fout_pre.open(DEBUG_FILE_DIR("mat_pre.txt"), std::ios::out); fout_out.open(DEBUG_FILE_DIR("mat_out.txt"), std::ios::out); + fout_nerf_color_.open(DEBUG_FILE_DIR("Colmap/color_poses.txt"), std::ios::out); + fout_nerf_depth_.open(DEBUG_FILE_DIR("Colmap/depth_poses.txt"), std::ios::out); } void LIVMapper::initializeSubscribersAndPublishers(ros::NodeHandle &nh, image_transport::ImageTransport &it) @@ -421,7 +506,16 @@ void LIVMapper::handleLIO() euler_cur = RotMtoEuler(_state.rot_end); geoQuat = tf::createQuaternionMsgFromRollPitchYaw(euler_cur(0), euler_cur(1), euler_cur(2)); publish_odometry(pubOdomAftMapped); - + /**********************************************************/ + Eigen::Isometry3d T_w_b = Eigen::Isometry3d::Identity(); + T_w_b.translation() = _state.pos_end; + T_w_b.linear() = _state.rot_end; + if(!img_cur_data_.empty()){ + std::unique_lock lock(data_mutex_); + cv::Mat image = img_cur_data_.clone(); + AllCloudAndPose_.push_back(NerfData(pcl_l_wait_pub, T_w_b, image)); + } + /**********************************************************/ double t3 = omp_get_wtime(); PointCloudXYZI::Ptr world_lidar(new PointCloudXYZI()); @@ -725,11 +819,11 @@ void LIVMapper::standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg) sig_buffer.notify_all(); } -void LIVMapper::livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg_in) +void LIVMapper::livox_pcl_cbk(const livox_ros_driver2::CustomMsg::ConstPtr &msg_in) { if (!lidar_en) return; mtx_buffer.lock(); - livox_ros_driver::CustomMsg::Ptr msg(new livox_ros_driver::CustomMsg(*msg_in)); + livox_ros_driver2::CustomMsg::Ptr msg(new livox_ros_driver2::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); diff --git a/src/preprocess.cpp b/src/preprocess.cpp index 322776b0..bc6d00b7 100755 --- a/src/preprocess.cpp +++ b/src/preprocess.cpp @@ -51,7 +51,7 @@ void Preprocess::set(bool feat_en, int lid_type, double bld, int pfilt_num) point_filter_num = pfilt_num; } -void Preprocess::process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out) +void Preprocess::process(const livox_ros_driver2::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out) { avia_handler(msg); *pcl_out = pl_surf; @@ -87,7 +87,7 @@ void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointClo *pcl_out = pl_surf; } -void Preprocess::avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg) +void Preprocess::avia_handler(const livox_ros_driver2::CustomMsg::ConstPtr &msg) { pl_surf.clear(); pl_corn.clear();