add the ability to generate M2Mapping data directly

.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"
+    }
+}

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

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
 

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.

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

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 
+

include/LIVMapper.h

@@ -20,7 +20,20 @@ which is included as part of this source code package.
 #include <image_transport/image_transport.h>
 #include <nav_msgs/Path.h>
 #include <vikit/camera_loader.h>
-
+#include <shared_mutex>
+#include <pcl/io/ply_io.h>
+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<NerfData> 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

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 <livox_ros_driver/CustomMsg.h>
+#include <livox_ros_driver2/CustomMsg.h>
 #include <pcl_conversions/pcl_conversions.h>
 
 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);

launch/mapping_mid360.launch

@@ -0,0 +1,20 @@
+<launch>
+<!-- Launch file for Livox AVIA LiDAR -->
+
+	<arg name="rviz" default="true" />
+
+	<rosparam command="load" file="$(find fast_livo)/config/mid360.yaml" />
+
+    <node pkg="fast_livo" type="fastlivo_mapping" name="laserMapping">
+		<rosparam file="$(find fast_livo)/config/camera_pinhole_mid360.yaml" />
+	</node>
+
+	<group if="$(arg rviz)">
+		<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_livo)/rviz_cfg/fast_livo2.rviz" />
+	</group>
+
+	<node pkg="image_transport" type="republish" name="republish" args="compressed in:=/camera/right/image raw out:=/camera/right/image" output="screen" respawn="true" />
+
+launch-prefix="gdb -ex run --args"
+launch-prefix="valgrind --leak-check=full"
+</launch>

package.xml

@@ -25,7 +25,7 @@
   <build_depend>sensor_msgs</build_depend>
   <build_depend>tf</build_depend>
   <build_depend>pcl_ros</build_depend>
-  <build_depend>livox_ros_driver</build_depend>
+  <build_depend>livox_ros_driver2</build_depend>
   <build_depend>message_generation</build_depend>
   <build_depend>vikit_common</build_depend>
   <build_depend>vikit_ros</build_depend>
@@ -39,7 +39,7 @@
   <run_depend>std_msgs</run_depend>
   <run_depend>tf</run_depend>
   <run_depend>pcl_ros</run_depend>
-  <run_depend>livox_ros_driver</run_depend>
+  <run_depend>livox_ros_driver2</run_depend>
   <run_depend>message_runtime</run_depend>
   <run_depend>vikit_common</run_depend>
   <run_depend>vikit_ros</run_depend>