├── CMakeLists.txt ├── README.md ├── config ├── l515.yaml └── velodyne.yaml ├── docs └── C3P_VoxelMap_Appendix.pdf ├── include ├── common_lib.h ├── so3_math.h └── voxel_map_util.hpp ├── launch ├── mapping_l515.launch └── mapping_velodyne.launch ├── msg ├── Pose6D.msg └── States.msg ├── package.xml ├── rviz_cfg └── c3p_voxelmap.rviz └── src ├── IMU_Processing.hpp ├── preprocess.cpp ├── preprocess.h └── voxelMapping.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(c3p_voxelmap) 3 | 4 | SET(CMAKE_BUILD_TYPE "RelWithDebInfo") 5 | 6 | ADD_COMPILE_OPTIONS(-std=c++14 ) 7 | ADD_COMPILE_OPTIONS(-std=c++14 ) 8 | set( CMAKE_CXX_FLAGS "-std=c++14 -O3" ) 9 | 10 | add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\") 11 | 12 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions" ) 13 | set(CMAKE_CXX_STANDARD 14) 14 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 15 | set(CMAKE_CXX_EXTENSIONS OFF) 16 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -pthread -std=c++0x -std=c++14 -fexceptions") 17 | 18 | message("Current CPU archtecture: ${CMAKE_SYSTEM_PROCESSOR}") 19 | if(CMAKE_SYSTEM_PROCESSOR MATCHES "(x86)|(X86)|(amd64)|(AMD64)" ) 20 | include(ProcessorCount) 21 | ProcessorCount(N) 22 | message("Processer number: ${N}") 23 | if(N GREATER 5) 24 | add_definitions(-DMP_EN) 25 | add_definitions(-DMP_PROC_NUM=4) 26 | message("core for MP: 4") 27 | elseif(N GREATER 3) 28 | math(EXPR PROC_NUM "${N} - 2") 29 | add_definitions(-DMP_EN) 30 | add_definitions(-DMP_PROC_NUM="${PROC_NUM}") 31 | message("core for MP: ${PROC_NUM}") 32 | else() 33 | add_definitions(-DMP_PROC_NUM=1) 34 | endif() 35 | else() 36 | add_definitions(-DMP_PROC_NUM=1) 37 | endif() 38 | 39 | 40 | find_package(catkin REQUIRED COMPONENTS 41 | geometry_msgs 42 | nav_msgs 43 | sensor_msgs 44 | roscpp 45 | rospy 46 | std_msgs 47 | pcl_ros 48 | tf 49 | livox_ros_driver 50 | message_generation 51 | eigen_conversions 52 | ) 53 | find_package(OpenMP QUIET) 54 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 55 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 56 | find_package(Eigen3 REQUIRED) 57 | find_package(PCL 1.7 REQUIRED) 58 | 59 | message(Eigen: ${EIGEN3_INCLUDE_DIR}) 60 | 61 | include_directories( 62 | ${catkin_INCLUDE_DIRS} 63 | ${EIGEN3_INCLUDE_DIR} 64 | ${PCL_INCLUDE_DIRS} 65 | ${PYTHON_INCLUDE_DIRS} 66 | include) 67 | 68 | add_message_files( 69 | FILES 70 | Pose6D.msg 71 | States.msg 72 | ) 73 | 74 | generate_messages( 75 | DEPENDENCIES 76 | geometry_msgs 77 | ) 78 | 79 | catkin_package( 80 | CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs message_runtime 81 | DEPENDS EIGEN3 PCL 82 | INCLUDE_DIRS 83 | ) 84 | 85 | add_executable(c3p_voxelmap src/voxelMapping.cpp src/preprocess.cpp) 86 | target_link_libraries(c3p_voxelmap ${catkin_LIBRARIES} ${PCL_LIBRARIES}) 87 | target_include_directories(c3p_voxelmap PRIVATE ${PYTHON_INCLUDE_DIRS}) -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # C³P-VoxelMap 2 | 3 | This repository implements **C³P-VoxelMap**, a compact, cumulative, and coalescible probabilistic voxel mapping method to enhance performance, accuracy, and memory efficiency in LiDAR odometry. Based on [VoxelMap](https://github.com/hku-mars/VoxelMap), our work reduces memory consumption by two strategies: 4 | 1. Compact point-free representation for probabilistic voxels and cumulative update of the planar uncertainty without caching original point clouds. 5 | 2. On-demand voxel merging by taking advantage of the geometric features in the real world, accumulating voxels in a locality-sensitive hash and triggers merging lazily. 6 | 7 | The paper is available on **arxiv**: 8 | [C³P-VoxelMap: Compact, Cumulative and Coalescible Probabilistic Voxel Mapping](https://arxiv.org/abs/2406.01195) 9 | 10 | ## Dependency 11 | 12 | ROS (tested on Noetic) 13 | 14 | PCL (>= 1.8) 15 | 16 | Eigen (>= 3.3.4) 17 | 18 | livox_ros_driver, follow [livox_ros_driver Installation](https://github.com/Livox-SDK/livox_ros_driver). 19 | 20 | ## Compilation 21 | 22 | Clone the repository and compile it by catkin_make: 23 | ``` 24 | mkdir -P ~/catkin_ws/src & cd ~/catkin_ws/src 25 | git clone https://github.com/deptrum/c3p-voxelmap.git 26 | cd .. 27 | catkin_make 28 | source devel/setup.bash 29 | ``` 30 | 31 | ## Running on Dataset 32 | 33 | To run on dataset (KITTI dataset for example), firstly edit configuration file ``` config/velodyne.yaml ```. 34 | 35 | Besides general parameters such as point cloud topic name, there are some extra configutations concerning on-demand voxel merging. To enable voxel merging, set ```voxel_merging/enable_voxel_merging``` to ```true```. The other parameters in ```voxel_merging``` can also be configured to adjust the effectiveness of on-demand voxel merging. 36 | 37 | Optionally, set ```visualization/pub_merged_voxel``` to ```true``` to visualize voxel merging results. Merged voxel planes are shown with the same color in visualization. 38 | 39 | After setting parameters, run the ROS package: 40 | ``` 41 | cd ~/catkin_ws 42 | source devel/setup.bash 43 | roslaunch c3p_voxelmap mapping_velodyne.launch 44 | ``` 45 | 46 | In the meanwhile, play rosbag and the visualization results will be shown in RViz window. 47 | 48 | ## Acknowledgments 49 | Thanks for [VoxelMap](https://github.com/hku-mars/VoxelMap). -------------------------------------------------------------------------------- /config/l515.yaml: -------------------------------------------------------------------------------- 1 | common: 2 | lid_topic: "/camera/depth/color/points" 3 | imu_topic: "/no_imu" 4 | 5 | preprocess: 6 | lidar_type: 3 # L515 LiDAR 7 | scan_line: 64 8 | blind: 0.1 9 | point_filter_num: 1 10 | calib_laser: false 11 | 12 | mapping: 13 | down_sample_size: 0.1 14 | max_iteration: 3 15 | voxel_size: 0.25 16 | max_layer: 2 17 | layer_point_size: [20, 10, 10, 5, 5] 18 | plannar_threshold: 0.01 19 | 20 | voxel_merging: 21 | enable_voxel_merging: true 22 | merge_theta_thresh: 0.2 23 | merge_dist_thresh: 0.1 24 | merge_cov_min_eigen_val_thresh: 0.002 25 | merge_x_coord_diff_thresh: 5.0 26 | merge_y_coord_diff_thresh: 5.0 27 | 28 | noise_model: 29 | ranging_cov: 0.05 30 | angle_cov: 0.25 31 | acc_cov_scale: 0.1 32 | gyr_cov_scale: 0.2 33 | 34 | imu: 35 | imu_en: false 36 | extrinsic_T: [ 0, 0, 0] 37 | extrinsic_R: [ 1, 0, 0, 38 | 0, 1, 0, 39 | 0, 0, 1] 40 | 41 | visualization: 42 | pub_voxel_map: false 43 | publish_max_voxel_layer: 1 # only publish 0,1 layer's plane 44 | pub_point_cloud: true 45 | dense_map_enable: false 46 | pub_point_cloud_skip: 1 # publish one points per five points 47 | pub_merged_voxel: true 48 | 49 | Result: 50 | write_kitti_log: false 51 | result_path: "" -------------------------------------------------------------------------------- /config/velodyne.yaml: -------------------------------------------------------------------------------- 1 | common: 2 | lid_topic: "/velodyne_points" 3 | imu_topic: "/imu_data" 4 | 5 | preprocess: 6 | lidar_type: 2 #1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for L515 LiDAR 7 | scan_line: 64 8 | blind: 1 9 | point_filter_num: 1 10 | calib_laser: true # true for KITTI Odometry dataset 11 | 12 | mapping: 13 | down_sample_size: 0.5 14 | max_iteration: 3 15 | voxel_size: 3.0 16 | max_layer: 3 # 4 layer, 0, 1, 2, 3 17 | layer_point_size: [5, 5, 5, 5, 5] 18 | plannar_threshold: 0.01 19 | 20 | voxel_merging: 21 | enable_voxel_merging: true 22 | merge_theta_thresh: 0.2 23 | merge_dist_thresh: 0.1 24 | merge_cov_min_eigen_val_thresh: 0.001 25 | merge_x_coord_diff_thresh: 10.0 26 | merge_y_coord_diff_thresh: 10.0 27 | 28 | noise_model: 29 | ranging_cov: 0.04 30 | angle_cov: 0.1 31 | acc_cov_scale: 1.0 32 | gyr_cov_scale: 0.5 33 | 34 | imu: 35 | imu_en: false 36 | extrinsic_T: [ 0, 0, 0] 37 | extrinsic_R: [ 1, 0, 0, 38 | 0, 1, 0, 39 | 0, 0, 1] 40 | 41 | visualization: 42 | pub_voxel_map: false 43 | publish_max_voxel_layer: 1 # only publish 0,1,2 layer's plane 44 | pub_point_cloud: true 45 | dense_map_enable: false 46 | pub_point_cloud_skip: 5 # publish one points per five points 47 | pub_merged_voxel: false 48 | 49 | Result: 50 | write_kitti_log: false 51 | result_path: "" -------------------------------------------------------------------------------- /docs/C3P_VoxelMap_Appendix.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/deptrum/c3p-voxelmap/69f9ac63938f7a55d048c48f6a2ec19c00730a69/docs/C3P_VoxelMap_Appendix.pdf -------------------------------------------------------------------------------- /include/common_lib.h: -------------------------------------------------------------------------------- 1 | #ifndef COMMON_LIB_H 2 | #define COMMON_LIB_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | using namespace std; 16 | using namespace Eigen; 17 | 18 | #define PI_M (3.14159265358) 19 | #define G_m_s2 (9.81) // Gravaty const in GuangDong/China 20 | #define DIM_STATE (18) // Dimension of states (Let Dim(SO(3)) = 3) 21 | #define DIM_PROC_N (12) // Dimension of process noise (Let Dim(SO(3)) = 3) 22 | #define CUBE_LEN (6.0) 23 | #define LIDAR_SP_LEN (2) 24 | // old init 25 | #define INIT_COV (0.0000001) 26 | #define NUM_MATCH_POINTS (5) 27 | #define MAX_MEAS_DIM (10000) 28 | 29 | #define VEC_FROM_ARRAY(v) v[0], v[1], v[2] 30 | #define MAT_FROM_ARRAY(v) v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8] 31 | #define CONSTRAIN(v, min, max) ((v > min) ? ((v < max) ? v : max) : min) 32 | #define ARRAY_FROM_EIGEN(mat) mat.data(), mat.data() + mat.rows() * mat.cols() 33 | #define STD_VEC_FROM_EIGEN(mat) \ 34 | vector(mat.data(), mat.data() + mat.rows() * mat.cols()) 35 | #define DEBUG_FILE_DIR(name) (string(string(ROOT_DIR) + "Log/" + name)) 36 | 37 | typedef voxel_map::Pose6D Pose6D; 38 | typedef pcl::PointXYZINormal PointType; 39 | typedef pcl::PointCloud PointCloudXYZI; 40 | typedef vector> PointVector; 41 | typedef Vector3d V3D; 42 | typedef Matrix3d M3D; 43 | typedef Vector3f V3F; 44 | typedef Matrix3f M3F; 45 | 46 | #define MD(a, b) Matrix 47 | #define VD(a) Matrix 48 | #define MF(a, b) Matrix 49 | #define VF(a) Matrix 50 | 51 | M3D Eye3d(M3D::Identity()); 52 | M3F Eye3f(M3F::Identity()); 53 | V3D Zero3d(0, 0, 0); 54 | V3F Zero3f(0, 0, 0); 55 | Vector3d Lidar_offset_to_IMU(0, 0, 0); 56 | 57 | struct MeasureGroup // Lidar data and imu dates for the curent process 58 | { 59 | MeasureGroup() { this->lidar.reset(new PointCloudXYZI()); }; 60 | double lidar_beg_time; 61 | PointCloudXYZI::Ptr lidar; 62 | deque imu; 63 | }; 64 | 65 | struct StatesGroup { 66 | StatesGroup() { 67 | this->rot_end = M3D::Identity(); 68 | this->pos_end = Zero3d; 69 | this->vel_end = Zero3d; 70 | this->bias_g = Zero3d; 71 | this->bias_a = Zero3d; 72 | this->gravity = Zero3d; 73 | this->cov = Matrix::Identity() * INIT_COV; 74 | }; 75 | 76 | StatesGroup(const StatesGroup& b) { 77 | this->rot_end = b.rot_end; 78 | this->pos_end = b.pos_end; 79 | this->vel_end = b.vel_end; 80 | this->bias_g = b.bias_g; 81 | this->bias_a = b.bias_a; 82 | this->gravity = b.gravity; 83 | this->cov = b.cov; 84 | }; 85 | 86 | StatesGroup& operator=(const StatesGroup& b) { 87 | this->rot_end = b.rot_end; 88 | this->pos_end = b.pos_end; 89 | this->vel_end = b.vel_end; 90 | this->bias_g = b.bias_g; 91 | this->bias_a = b.bias_a; 92 | this->gravity = b.gravity; 93 | this->cov = b.cov; 94 | return *this; 95 | }; 96 | 97 | StatesGroup operator+(const Matrix& state_add) { 98 | StatesGroup a; 99 | a.rot_end = this->rot_end * Exp(state_add(0, 0), state_add(1, 0), state_add(2, 0)); 100 | a.pos_end = this->pos_end + state_add.block<3, 1>(3, 0); 101 | a.vel_end = this->vel_end + state_add.block<3, 1>(6, 0); 102 | a.bias_g = this->bias_g + state_add.block<3, 1>(9, 0); 103 | a.bias_a = this->bias_a + state_add.block<3, 1>(12, 0); 104 | a.gravity = this->gravity + state_add.block<3, 1>(15, 0); 105 | a.cov = this->cov; 106 | return a; 107 | }; 108 | 109 | StatesGroup& operator+=(const Matrix& state_add) { 110 | this->rot_end = this->rot_end * Exp(state_add(0, 0), state_add(1, 0), state_add(2, 0)); 111 | this->pos_end += state_add.block<3, 1>(3, 0); 112 | this->vel_end += state_add.block<3, 1>(6, 0); 113 | this->bias_g += state_add.block<3, 1>(9, 0); 114 | this->bias_a += state_add.block<3, 1>(12, 0); 115 | this->gravity += state_add.block<3, 1>(15, 0); 116 | return *this; 117 | }; 118 | 119 | Matrix operator-(const StatesGroup& b) { 120 | Matrix a; 121 | M3D rotd(b.rot_end.transpose() * this->rot_end); 122 | a.block<3, 1>(0, 0) = Log(rotd); 123 | a.block<3, 1>(3, 0) = this->pos_end - b.pos_end; 124 | a.block<3, 1>(6, 0) = this->vel_end - b.vel_end; 125 | a.block<3, 1>(9, 0) = this->bias_g - b.bias_g; 126 | a.block<3, 1>(12, 0) = this->bias_a - b.bias_a; 127 | a.block<3, 1>(15, 0) = this->gravity - b.gravity; 128 | return a; 129 | }; 130 | 131 | void resetpose() { 132 | this->rot_end = M3D::Identity(); 133 | this->pos_end = Zero3d; 134 | this->vel_end = Zero3d; 135 | } 136 | 137 | M3D rot_end; // the estimated attitude (rotation matrix) at the end lidar 138 | // point 139 | V3D pos_end; // the estimated position at the end lidar point (world frame) 140 | V3D vel_end; // the estimated velocity at the end lidar point (world frame) 141 | V3D bias_g; // gyroscope bias 142 | V3D bias_a; // accelerator bias 143 | V3D gravity; // the estimated gravity acceleration 144 | Matrix cov; // states covariance 145 | }; 146 | 147 | template 148 | T rad2deg(T radians) { 149 | return radians * 180.0 / PI_M; 150 | } 151 | 152 | template 153 | T deg2rad(T degrees) { 154 | return degrees * PI_M / 180.0; 155 | } 156 | 157 | template 158 | auto set_pose6d(const double t, 159 | const Matrix& a, 160 | const Matrix& g, 161 | const Matrix& v, 162 | const Matrix& p, 163 | const Matrix& R) { 164 | Pose6D rot_kp; 165 | rot_kp.offset_time = t; 166 | for (int i = 0; i < 3; i++) { 167 | rot_kp.acc[i] = a(i); 168 | rot_kp.gyr[i] = g(i); 169 | rot_kp.vel[i] = v(i); 170 | rot_kp.pos[i] = p(i); 171 | for (int j = 0; j < 3; j++) 172 | rot_kp.rot[i * 3 + j] = R(i, j); 173 | } 174 | // Map(rot_kp.rot, 3,3) = R; 175 | return move(rot_kp); 176 | } 177 | 178 | /* comment 179 | plane equation: Ax + By + Cz + D = 0 180 | convert to: A/D*x + B/D*y + C/D*z = -1 181 | solve: A0*x0 = b0 182 | where A0_i = [x_i, y_i, z_i], x0 = [A/D, B/D, C/D]^T, b0 = [-1, ..., -1]^T 183 | normvec: normalized x0 184 | */ 185 | template 186 | bool esti_normvector(Matrix& normvec, 187 | const PointVector& point, 188 | const T& threshold, 189 | const int& point_num) { 190 | MatrixXf A(point_num, 3); 191 | MatrixXf b(point_num, 1); 192 | b.setOnes(); 193 | b *= -1.0f; 194 | 195 | for (int j = 0; j < point_num; j++) { 196 | A(j, 0) = point[j].x; 197 | A(j, 1) = point[j].y; 198 | A(j, 2) = point[j].z; 199 | } 200 | normvec = A.colPivHouseholderQr().solve(b); 201 | 202 | for (int j = 0; j < point_num; j++) { 203 | if (fabs(normvec(0) * point[j].x + normvec(1) * point[j].y + normvec(2) * point[j].z + 1.0f) > 204 | threshold) { 205 | return false; 206 | } 207 | } 208 | 209 | normvec.normalize(); 210 | return true; 211 | } 212 | 213 | template 214 | bool esti_plane(Matrix& pca_result, const PointVector& point, const T& threshold) { 215 | Matrix A; 216 | Matrix b; 217 | b.setOnes(); 218 | b *= -1.0f; 219 | 220 | for (int j = 0; j < NUM_MATCH_POINTS; j++) { 221 | A(j, 0) = point[j].x; 222 | A(j, 1) = point[j].y; 223 | A(j, 2) = point[j].z; 224 | } 225 | 226 | Matrix normvec = A.colPivHouseholderQr().solve(b); 227 | 228 | for (int j = 0; j < NUM_MATCH_POINTS; j++) { 229 | if (fabs(normvec(0) * point[j].x + normvec(1) * point[j].y + normvec(2) * point[j].z + 1.0f) > 230 | threshold) { 231 | return false; 232 | } 233 | } 234 | 235 | T n = normvec.norm(); 236 | pca_result(0) = normvec(0) / n; 237 | pca_result(1) = normvec(1) / n; 238 | pca_result(2) = normvec(2) / n; 239 | pca_result(3) = 1.0 / n; 240 | return true; 241 | } 242 | 243 | #endif -------------------------------------------------------------------------------- /include/so3_math.h: -------------------------------------------------------------------------------- 1 | #ifndef SO3_MATH_H 2 | #define SO3_MATH_H 3 | 4 | #include 5 | #include 6 | 7 | // #include 8 | 9 | #define SKEW_SYM_MATRX(v) 0.0, -v[2], v[1], v[2], 0.0, -v[0], -v[1], v[0], 0.0 10 | 11 | template 12 | Eigen::Matrix Exp(const Eigen::Matrix&& ang) { 13 | T ang_norm = ang.norm(); 14 | Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); 15 | if (ang_norm > 0.0000001) { 16 | Eigen::Matrix r_axis = ang / ang_norm; 17 | Eigen::Matrix K; 18 | K << SKEW_SYM_MATRX(r_axis); 19 | /// Roderigous Tranformation 20 | return Eye3 + std::sin(ang_norm) * K + (1.0 - std::cos(ang_norm)) * K * K; 21 | } else { 22 | return Eye3; 23 | } 24 | } 25 | 26 | template 27 | Eigen::Matrix Exp(const Eigen::Matrix& ang_vel, const Ts& dt) { 28 | T ang_vel_norm = ang_vel.norm(); 29 | Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); 30 | 31 | if (ang_vel_norm > 0.0000001) { 32 | Eigen::Matrix r_axis = ang_vel / ang_vel_norm; 33 | Eigen::Matrix K; 34 | 35 | K << SKEW_SYM_MATRX(r_axis); 36 | 37 | T r_ang = ang_vel_norm * dt; 38 | 39 | /// Roderigous Tranformation 40 | return Eye3 + std::sin(r_ang) * K + (1.0 - std::cos(r_ang)) * K * K; 41 | } else { 42 | return Eye3; 43 | } 44 | } 45 | 46 | template 47 | Eigen::Matrix Exp(const T& v1, const T& v2, const T& v3) { 48 | T&& norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3); 49 | Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); 50 | if (norm > 0.00001) { 51 | T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm}; 52 | Eigen::Matrix K; 53 | K << SKEW_SYM_MATRX(r_ang); 54 | 55 | /// Roderigous Tranformation 56 | return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K; 57 | } else { 58 | return Eye3; 59 | } 60 | } 61 | 62 | /* Logrithm of a Rotation Matrix */ 63 | template 64 | Eigen::Matrix Log(const Eigen::Matrix& R) { 65 | T theta = (R.trace() > 3.0 - 1e-6) ? 0.0 : std::acos(0.5 * (R.trace() - 1)); 66 | Eigen::Matrix K(R(2, 1) - R(1, 2), R(0, 2) - R(2, 0), R(1, 0) - R(0, 1)); 67 | return (std::abs(theta) < 0.001) ? (0.5 * K) : (0.5 * theta / std::sin(theta) * K); 68 | } 69 | 70 | template 71 | Eigen::Matrix RotMtoEuler(const Eigen::Matrix& rot) { 72 | T sy = sqrt(rot(0, 0) * rot(0, 0) + rot(1, 0) * rot(1, 0)); 73 | bool singular = sy < 1e-6; 74 | T x, y, z; 75 | if (!singular) { 76 | x = atan2(rot(2, 1), rot(2, 2)); 77 | y = atan2(-rot(2, 0), sy); 78 | z = atan2(rot(1, 0), rot(0, 0)); 79 | } else { 80 | x = atan2(-rot(1, 2), rot(1, 1)); 81 | y = atan2(-rot(2, 0), sy); 82 | z = 0; 83 | } 84 | Eigen::Matrix ang(x, y, z); 85 | return ang; 86 | } 87 | 88 | inline Eigen::Matrix3d SkewSymmetricMatrix(const Eigen::Vector3d& v) { 89 | Eigen::Matrix3d skew_v; 90 | skew_v << 0.0, -v[2], v[1], v[2], 0.0, -v[0], -v[1], v[0], 0.0; 91 | 92 | return skew_v; 93 | } 94 | 95 | Eigen::Matrix3d Exp(const Eigen::Vector3d& ang) { 96 | double ang_norm = ang.norm(); 97 | 98 | Eigen::Matrix3d I33(Eigen::Matrix3d::Identity()); 99 | if (ang_norm >= std::numeric_limits::epsilon()) { 100 | Eigen::Matrix r_axis = ang / ang_norm; 101 | Eigen::Matrix K; 102 | K << SkewSymmetricMatrix(r_axis); 103 | /// Roderigous Tranformation 104 | return I33 + std::sin(ang_norm) * K + (1.0 - std::cos(ang_norm)) * K * K; 105 | } 106 | 107 | return I33; 108 | } 109 | 110 | #endif 111 | -------------------------------------------------------------------------------- /include/voxel_map_util.hpp: -------------------------------------------------------------------------------- 1 | #ifndef VOXEL_MAP_UTIL_HPP 2 | #define VOXEL_MAP_UTIL_HPP 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include "common_lib.h" 18 | #include "omp.h" 19 | 20 | #define HASH_P 116101 21 | #define MAX_N 10000000000 22 | 23 | static int plane_id = 0; 24 | static int merged_plane_idx = 0; 25 | int voxel_nums = 0; 26 | int merged_voxel_nums = 0; 27 | 28 | // a point to plane matching structure 29 | typedef struct ptpl { 30 | Eigen::Vector3d point; 31 | Eigen::Vector3d normal; 32 | Eigen::Vector3d center; 33 | Eigen::Matrix plane_cov; 34 | double d; 35 | int layer; 36 | } ptpl; 37 | 38 | // 3D point with covariance 39 | typedef struct pointWithCov { 40 | Eigen::Vector3d point; 41 | Eigen::Vector3d point_world; 42 | Eigen::Matrix3d cov; 43 | } pointWithCov; 44 | 45 | struct Plane { 46 | Eigen::Vector3d center; 47 | Eigen::Vector3d normal; 48 | Eigen::Vector3d y_normal; 49 | Eigen::Vector3d x_normal; 50 | Eigen::Matrix3d covariance; 51 | Eigen::Matrix plane_cov; 52 | float radius = 0; 53 | float min_eigen_value = 1; 54 | float mid_eigen_value = 1; 55 | float max_eigen_value = 1; 56 | float d = 0; 57 | int points_size = 0; 58 | 59 | bool is_plane = false; 60 | bool is_init = false; 61 | int id = -1; 62 | int merge_plane_id = -1; 63 | // is_update and last_update_points_size are only for publish plane 64 | bool is_update = false; 65 | int last_update_points_size = 0; 66 | 67 | Plane() = default; 68 | }; 69 | 70 | typedef struct PointFreeParams { 71 | Eigen::Matrix sigma_p_pt; 72 | Eigen::Vector3d sigma_p; 73 | size_t point_nums; 74 | Eigen::Matrix sigma_cov11_ppt; 75 | Eigen::Matrix sigma_cov12_ppt; 76 | Eigen::Matrix sigma_cov13_ppt; 77 | Eigen::Matrix sigma_cov22_ppt; 78 | Eigen::Matrix sigma_cov23_ppt; 79 | Eigen::Matrix sigma_cov33_ppt; 80 | Eigen::Matrix3d sigma_cov_e1_pt; 81 | Eigen::Matrix3d sigma_cov_e2_pt; 82 | Eigen::Matrix3d sigma_cov_e3_pt; 83 | Eigen::Matrix sigma_cov; 84 | 85 | void Initialize() { 86 | sigma_p_pt.setZero(); 87 | sigma_p.setZero(); 88 | point_nums = 0; 89 | sigma_cov11_ppt.setZero(); 90 | sigma_cov12_ppt.setZero(); 91 | sigma_cov13_ppt.setZero(); 92 | sigma_cov22_ppt.setZero(); 93 | sigma_cov23_ppt.setZero(); 94 | sigma_cov33_ppt.setZero(); 95 | sigma_cov_e1_pt.setZero(); 96 | sigma_cov_e2_pt.setZero(); 97 | sigma_cov_e3_pt.setZero(); 98 | sigma_cov.setZero(); 99 | } 100 | } PointFreeParams; 101 | 102 | class PointCluster { 103 | public: 104 | Eigen::Matrix3d P; 105 | Eigen::Vector3d v; 106 | int N; 107 | 108 | PointCluster() { 109 | P.setZero(); 110 | v.setZero(); 111 | N = 0; 112 | } 113 | 114 | void Clear() { 115 | P.setZero(); 116 | v.setZero(); 117 | N = 0; 118 | } 119 | void Push(const Eigen::Vector3d& vec) { 120 | N++; 121 | P += vec * vec.transpose(); 122 | v += vec; 123 | } 124 | 125 | Eigen::Matrix3d Cov() { 126 | Eigen::Vector3d center = v / N; 127 | return P / N - center * center.transpose(); 128 | } 129 | 130 | PointCluster operator+(const PointCluster& b) const { 131 | PointCluster res; 132 | 133 | res.P = this->P + b.P; 134 | res.v = this->v + b.v; 135 | res.N = this->N + b.N; 136 | 137 | return res; 138 | } 139 | 140 | PointCluster& operator+=(const PointCluster& sigv) { 141 | this->P += sigv.P; 142 | this->v += sigv.v; 143 | this->N += sigv.N; 144 | 145 | return *this; 146 | } 147 | }; 148 | 149 | class VOXEL_LOC { 150 | public: 151 | int64_t x, y, z; 152 | 153 | VOXEL_LOC(int64_t vx = 0, int64_t vy = 0, int64_t vz = 0) : x(vx), y(vy), z(vz) {} 154 | 155 | bool operator==(const VOXEL_LOC& other) const { 156 | return (x == other.x && y == other.y && z == other.z); 157 | } 158 | }; 159 | 160 | struct PlaneDesc { 161 | public: 162 | int64_t theta_bin; 163 | int64_t phi_bin; 164 | int64_t d_bin; 165 | int64_t x_bin; 166 | int64_t y_bin; 167 | 168 | PlaneDesc(const Plane* const plane, 169 | double merge_x_coord_diff_thresh, 170 | double merge_y_coord_diff_thresh) { 171 | double phi = std::asin(plane->normal[2]); 172 | double theta = std::atan2(plane->normal[1], plane->normal[0]); 173 | 174 | Eigen::Matrix3d R = (Eigen::AngleAxisd(theta, Vector3d::UnitZ()) * 175 | Eigen::AngleAxisd(-phi, Vector3d::UnitY())) 176 | .toRotationMatrix(); 177 | 178 | if (phi < std::numeric_limits::epsilon()) { 179 | phi += M_PI; 180 | } 181 | 182 | if (theta < std::numeric_limits::epsilon()) { 183 | theta += 2 * M_PI; 184 | } 185 | phi *= 57.29578; 186 | theta *= 57.29578; 187 | 188 | float d = 10.f * plane->d; 189 | 190 | theta_bin = static_cast(phi / 10.f); 191 | phi_bin = static_cast(theta / 5.f); 192 | d_bin = static_cast(d / 10.f); 193 | 194 | Eigen::Vector3d coord_in_xy_plane = R * plane->center; 195 | 196 | x_bin = static_cast(coord_in_xy_plane(1) / merge_x_coord_diff_thresh); 197 | y_bin = static_cast(coord_in_xy_plane(2) / merge_y_coord_diff_thresh); 198 | } 199 | bool operator==(const PlaneDesc& other) const { 200 | return (theta_bin == other.theta_bin && phi_bin == other.phi_bin && d_bin == other.d_bin && 201 | x_bin == other.x_bin && y_bin == other.y_bin); 202 | } 203 | }; 204 | 205 | struct NodeLoc { 206 | public: 207 | int64_t x; 208 | int64_t y; 209 | int64_t z; 210 | int64_t leaf_num; 211 | NodeLoc() : x(0), y(0), z(0), leaf_num(0) {} 212 | NodeLoc(int64_t x_, int64_t y_, int64_t z_, int64_t leaf_num_) : 213 | x(x_), 214 | y(y_), 215 | z(z_), 216 | leaf_num(leaf_num_){}; 217 | 218 | NodeLoc(const VOXEL_LOC& v, const std::vector& leaf_path) : x(v.x), y(v.y), z(v.z) { 219 | leaf_num = 0; 220 | 221 | if (!leaf_path.empty()) { 222 | for (int i = leaf_path.size() - 1; i >= 0; i--) { 223 | leaf_num = 10 * leaf_num + leaf_path[i] + 1; 224 | } 225 | } 226 | } 227 | 228 | bool operator==(const NodeLoc& other) const { 229 | return (x == other.x) && (y == other.y) && (z == other.z) && (leaf_num == other.leaf_num); 230 | } 231 | }; 232 | 233 | // Hash value 234 | namespace std { 235 | template<> 236 | struct hash { 237 | int64_t operator()(const VOXEL_LOC& s) const { 238 | using std::hash; 239 | using std::size_t; 240 | return ((((s.z) * HASH_P) % MAX_N + (s.y)) * HASH_P) % MAX_N + (s.x); 241 | } 242 | }; 243 | 244 | template<> 245 | struct hash { 246 | int64_t operator()(const PlaneDesc& plane) const { 247 | using std::hash; 248 | using std::size_t; 249 | return ((((((((plane.theta_bin) * HASH_P) % MAX_N + (plane.phi_bin)) * HASH_P) % MAX_N + 250 | (plane.d_bin)) * 251 | HASH_P) % 252 | MAX_N + 253 | (plane.x_bin)) * 254 | HASH_P) % 255 | MAX_N + 256 | (plane.y_bin); 257 | } 258 | }; 259 | 260 | template<> 261 | struct hash { 262 | std::size_t operator()(const NodeLoc& c) const { 263 | return ((((((c.z) * HASH_P) % MAX_N + (c.y)) * HASH_P) % MAX_N + (c.x)) * HASH_P) % MAX_N + 264 | c.leaf_num; 265 | } 266 | }; 267 | } // namespace std 268 | 269 | struct UnionPlane { 270 | NodeLoc root_node_loc; 271 | std::unordered_set plane_bucket; 272 | }; 273 | 274 | class OctoTree { 275 | public: 276 | std::vector temp_points_; // all points in an octo tree 277 | std::vector new_points_; // new points in an octo tree 278 | Plane* plane_ptr_; 279 | PointFreeParams* point_free_params_; 280 | int max_layer_; 281 | bool indoor_mode_; 282 | int layer_; 283 | int octo_state_; // 0 is end of tree, 1 is not 284 | OctoTree* leaves_[8]; 285 | double voxel_center_[3]; // x, y, z 286 | std::vector layer_point_size_; 287 | float quater_length_; 288 | float planer_threshold_; 289 | int max_plane_update_threshold_; 290 | bool init_octo_; 291 | PointCluster point_cluster_; 292 | uint32_t last_update_frame_; 293 | OctoTree(int max_layer, int layer, std::vector layer_point_size, float planer_threshold) : 294 | max_layer_(max_layer), 295 | layer_(layer), 296 | layer_point_size_(layer_point_size), 297 | planer_threshold_(planer_threshold) { 298 | temp_points_.clear(); 299 | octo_state_ = 0; 300 | init_octo_ = false; 301 | max_plane_update_threshold_ = layer_point_size_[layer_]; 302 | for (int i = 0; i < 8; i++) { 303 | leaves_[i] = nullptr; 304 | } 305 | point_free_params_ = nullptr; 306 | plane_ptr_ = new Plane; 307 | last_update_frame_ = 0; 308 | } 309 | 310 | ~OctoTree() { 311 | delete plane_ptr_; 312 | delete point_free_params_; 313 | for (size_t i = 0; i < 8; i++) { 314 | delete leaves_[i]; 315 | } 316 | } 317 | 318 | int GetLeafNums() { 319 | int res = 0; 320 | for (size_t i = 0; i < 8; i++) { 321 | if (leaves_[i] != nullptr) { 322 | res += leaves_[i]->GetLeafNums(); 323 | } 324 | } 325 | return res; 326 | } 327 | 328 | void MergeVoxelPlane(const OctoTree* const node) { 329 | point_cluster_ += node->point_cluster_; 330 | 331 | point_free_params_->sigma_p_pt += node->point_free_params_->sigma_p_pt; 332 | point_free_params_->sigma_p += node->point_free_params_->sigma_p; 333 | point_free_params_->point_nums += node->point_free_params_->point_nums; 334 | point_free_params_->sigma_cov11_ppt += node->point_free_params_->sigma_cov11_ppt; 335 | point_free_params_->sigma_cov12_ppt += node->point_free_params_->sigma_cov12_ppt; 336 | point_free_params_->sigma_cov13_ppt += node->point_free_params_->sigma_cov13_ppt; 337 | point_free_params_->sigma_cov22_ppt += node->point_free_params_->sigma_cov22_ppt; 338 | point_free_params_->sigma_cov23_ppt += node->point_free_params_->sigma_cov23_ppt; 339 | point_free_params_->sigma_cov33_ppt += node->point_free_params_->sigma_cov33_ppt; 340 | point_free_params_->sigma_cov_e1_pt += node->point_free_params_->sigma_cov_e1_pt; 341 | point_free_params_->sigma_cov_e2_pt += node->point_free_params_->sigma_cov_e2_pt; 342 | point_free_params_->sigma_cov_e3_pt += node->point_free_params_->sigma_cov_e3_pt; 343 | point_free_params_->sigma_cov += node->point_free_params_->sigma_cov; 344 | 345 | plane_ptr_->center = point_cluster_.v / point_cluster_.N; 346 | plane_ptr_->covariance = point_cluster_.Cov(); 347 | Eigen::SelfAdjointEigenSolver solver(plane_ptr_->covariance); 348 | 349 | Eigen::Vector3d eigen_value = solver.eigenvalues(); 350 | Eigen::Matrix3d eigen_vector = solver.eigenvectors(); 351 | 352 | plane_ptr_->normal = eigen_vector.col(0); 353 | 354 | plane_ptr_->min_eigen_value = eigen_value(0); 355 | plane_ptr_->mid_eigen_value = eigen_value(1); 356 | plane_ptr_->max_eigen_value = eigen_value(2); 357 | plane_ptr_->radius = std::sqrt(eigen_value(2)); 358 | plane_ptr_->d = -(plane_ptr_->normal.dot(plane_ptr_->center)); 359 | plane_ptr_->points_size = point_cluster_.N; 360 | 361 | std::vector B(3); 362 | // Jacobian of the normal of plane w.r.t the world point 363 | for (int m = 0; m < 3; m++) { 364 | if (m != 0) { 365 | B[m] = (eigen_vector.col(m) * eigen_vector.col(0).transpose() + 366 | eigen_vector.col(0) * eigen_vector.col(m).transpose()) / 367 | (plane_ptr_->points_size * (eigen_value[0] - eigen_value[m])); 368 | } else { 369 | B[m].setZero(); 370 | } 371 | } 372 | CalculatePointFreeVoxelPlaneCovariance(plane_ptr_->plane_cov, 373 | plane_ptr_->center, 374 | B, 375 | eigen_vector, 376 | 0); 377 | } 378 | 379 | void UpdatePointFreeParams(const pointWithCov& pv) { 380 | const auto& pi = pv.point; 381 | const auto& cov = pv.cov; 382 | 383 | Eigen::Vector3d e1(1.0, 0.0, 0.0); 384 | Eigen::Vector3d e2(0.0, 1.0, 0.0); 385 | Eigen::Vector3d e3(0.0, 0.0, 1.0); 386 | 387 | point_free_params_->point_nums++; 388 | point_free_params_->sigma_p += pi; 389 | 390 | auto p_pt = pi * pi.transpose(); 391 | point_free_params_->sigma_p_pt(0) += p_pt(0, 0); 392 | point_free_params_->sigma_p_pt(1) += p_pt(0, 1); 393 | point_free_params_->sigma_p_pt(2) += p_pt(0, 2); 394 | point_free_params_->sigma_p_pt(3) += p_pt(1, 1); 395 | point_free_params_->sigma_p_pt(4) += p_pt(1, 2); 396 | point_free_params_->sigma_p_pt(5) += p_pt(2, 2); 397 | 398 | Eigen::Matrix3d cov11_ppt = cov(0, 0) * pi * pi.transpose(); 399 | point_free_params_->sigma_cov11_ppt(0) += cov11_ppt(0, 0); 400 | point_free_params_->sigma_cov11_ppt(1) += cov11_ppt(0, 1); 401 | point_free_params_->sigma_cov11_ppt(2) += cov11_ppt(0, 2); 402 | point_free_params_->sigma_cov11_ppt(3) += cov11_ppt(1, 1); 403 | point_free_params_->sigma_cov11_ppt(4) += cov11_ppt(1, 2); 404 | point_free_params_->sigma_cov11_ppt(5) += cov11_ppt(2, 2); 405 | 406 | Eigen::Matrix3d cov12_ppt = cov(0, 1) * pi * pi.transpose(); 407 | point_free_params_->sigma_cov12_ppt(0) += cov12_ppt(0, 0); 408 | point_free_params_->sigma_cov12_ppt(1) += cov12_ppt(0, 1); 409 | point_free_params_->sigma_cov12_ppt(2) += cov12_ppt(0, 2); 410 | point_free_params_->sigma_cov12_ppt(3) += cov12_ppt(1, 1); 411 | point_free_params_->sigma_cov12_ppt(4) += cov12_ppt(1, 2); 412 | point_free_params_->sigma_cov12_ppt(5) += cov12_ppt(2, 2); 413 | 414 | Eigen::Matrix3d cov13_ppt = cov(0, 2) * pi * pi.transpose(); 415 | point_free_params_->sigma_cov13_ppt(0) += cov13_ppt(0, 0); 416 | point_free_params_->sigma_cov13_ppt(1) += cov13_ppt(0, 1); 417 | point_free_params_->sigma_cov13_ppt(2) += cov13_ppt(0, 2); 418 | point_free_params_->sigma_cov13_ppt(3) += cov13_ppt(1, 1); 419 | point_free_params_->sigma_cov13_ppt(4) += cov13_ppt(1, 2); 420 | point_free_params_->sigma_cov13_ppt(5) += cov13_ppt(2, 2); 421 | 422 | Eigen::Matrix3d cov22_ppt = cov(1, 1) * pi * pi.transpose(); 423 | point_free_params_->sigma_cov22_ppt(0) += cov22_ppt(0, 0); 424 | point_free_params_->sigma_cov22_ppt(1) += cov22_ppt(0, 1); 425 | point_free_params_->sigma_cov22_ppt(2) += cov22_ppt(0, 2); 426 | point_free_params_->sigma_cov22_ppt(3) += cov22_ppt(1, 1); 427 | point_free_params_->sigma_cov22_ppt(4) += cov22_ppt(1, 2); 428 | point_free_params_->sigma_cov22_ppt(5) += cov22_ppt(2, 2); 429 | 430 | Eigen::Matrix3d cov23_ppt = cov(1, 2) * pi * pi.transpose(); 431 | point_free_params_->sigma_cov23_ppt(0) += cov23_ppt(0, 0); 432 | point_free_params_->sigma_cov23_ppt(1) += cov23_ppt(0, 1); 433 | point_free_params_->sigma_cov23_ppt(2) += cov23_ppt(0, 2); 434 | point_free_params_->sigma_cov23_ppt(3) += cov23_ppt(1, 1); 435 | point_free_params_->sigma_cov23_ppt(4) += cov23_ppt(1, 2); 436 | point_free_params_->sigma_cov23_ppt(5) += cov23_ppt(2, 2); 437 | 438 | Eigen::Matrix3d cov33_ppt = cov(2, 2) * pi * pi.transpose(); 439 | point_free_params_->sigma_cov33_ppt(0) += cov33_ppt(0, 0); 440 | point_free_params_->sigma_cov33_ppt(1) += cov33_ppt(0, 1); 441 | point_free_params_->sigma_cov33_ppt(2) += cov33_ppt(0, 2); 442 | point_free_params_->sigma_cov33_ppt(3) += cov33_ppt(1, 1); 443 | point_free_params_->sigma_cov33_ppt(4) += cov33_ppt(1, 2); 444 | point_free_params_->sigma_cov33_ppt(5) += cov33_ppt(2, 2); 445 | 446 | point_free_params_->sigma_cov_e1_pt += cov * e1 * pi.transpose(); 447 | point_free_params_->sigma_cov_e2_pt += cov * e2 * pi.transpose(); 448 | point_free_params_->sigma_cov_e3_pt += cov * e3 * pi.transpose(); 449 | 450 | point_free_params_->sigma_cov(0) += cov(0, 0); 451 | point_free_params_->sigma_cov(1) += cov(0, 1); 452 | point_free_params_->sigma_cov(2) += cov(0, 2); 453 | point_free_params_->sigma_cov(3) += cov(1, 1); 454 | point_free_params_->sigma_cov(4) += cov(1, 2); 455 | point_free_params_->sigma_cov(5) += cov(2, 2); 456 | } 457 | 458 | bool UpdateMergedPlanePointFreeParams(const pointWithCov& pv) { 459 | const auto& pi = pv.point; 460 | const auto& cov = pv.cov; 461 | 462 | auto p_pt = pi * pi.transpose(); 463 | Eigen::Matrix sigma_p_pt; 464 | sigma_p_pt(0) = point_free_params_->sigma_p_pt(0) + p_pt(0, 0); 465 | sigma_p_pt(1) = point_free_params_->sigma_p_pt(1) + p_pt(0, 1); 466 | sigma_p_pt(2) = point_free_params_->sigma_p_pt(2) + p_pt(0, 2); 467 | sigma_p_pt(3) = point_free_params_->sigma_p_pt(3) + p_pt(1, 1); 468 | sigma_p_pt(4) = point_free_params_->sigma_p_pt(4) + p_pt(1, 2); 469 | sigma_p_pt(5) = point_free_params_->sigma_p_pt(5) + p_pt(2, 2); 470 | 471 | Eigen::Vector3d sigma_p = point_free_params_->sigma_p + pi; 472 | 473 | Eigen::Matrix3d sigma_p_pt_mat; 474 | sigma_p_pt_mat(0, 0) = sigma_p_pt(0); 475 | sigma_p_pt_mat(0, 1) = sigma_p_pt(1); 476 | sigma_p_pt_mat(0, 2) = sigma_p_pt(2); 477 | sigma_p_pt_mat(1, 0) = sigma_p_pt(1); 478 | sigma_p_pt_mat(1, 1) = sigma_p_pt(3); 479 | sigma_p_pt_mat(1, 2) = sigma_p_pt(4); 480 | sigma_p_pt_mat(2, 0) = sigma_p_pt(2); 481 | sigma_p_pt_mat(2, 1) = sigma_p_pt(4); 482 | sigma_p_pt_mat(2, 2) = sigma_p_pt(5); 483 | 484 | int cur_point_size = point_free_params_->point_nums + 1; 485 | Eigen::Vector3d cur_center = sigma_p / cur_point_size; 486 | Eigen::Matrix3d cur_covariance = sigma_p_pt_mat / cur_point_size - 487 | cur_center * cur_center.transpose(); 488 | 489 | Eigen::EigenSolver es(cur_covariance); 490 | Eigen::Matrix3cd evecs = es.eigenvectors(); 491 | Eigen::Vector3cd evals = es.eigenvalues(); 492 | Eigen::Vector3d evalsReal; 493 | evalsReal = evals.real(); 494 | Eigen::Matrix3d::Index evalsMin, evalsMax; 495 | evalsReal.rowwise().sum().minCoeff(&evalsMin); 496 | evalsReal.rowwise().sum().maxCoeff(&evalsMax); 497 | int evalsMid = 3 - evalsMin - evalsMax; 498 | Eigen::Vector3d evecMin = evecs.real().col(evalsMin); 499 | Eigen::Vector3d evecMid = evecs.real().col(evalsMid); 500 | Eigen::Vector3d evecMax = evecs.real().col(evalsMax); 501 | 502 | if (evalsReal(evalsMin) < planer_threshold_) { 503 | UpdatePointFreeParams(pv); 504 | 505 | return false; 506 | } 507 | 508 | return true; 509 | } 510 | 511 | void CalculatePointFreeVoxelPlaneCovariance(Eigen::Matrix& plane_cov, 512 | const Eigen::Vector3d& center, 513 | const std::vector& B, 514 | const Eigen::Matrix3d& J_n_pw, 515 | const int& min_eval_idx) { 516 | plane_cov.setZero(); 517 | Eigen::Vector3d e1(1.0, 0.0, 0.0); 518 | Eigen::Vector3d e2(0.0, 1.0, 0.0); 519 | Eigen::Vector3d e3(0.0, 0.0, 1.0); 520 | 521 | Eigen::Matrix3d sigma_cov11_ppt_mat; 522 | sigma_cov11_ppt_mat(0, 0) = point_free_params_->sigma_cov11_ppt(0); 523 | sigma_cov11_ppt_mat(0, 1) = point_free_params_->sigma_cov11_ppt(1); 524 | sigma_cov11_ppt_mat(0, 2) = point_free_params_->sigma_cov11_ppt(2); 525 | sigma_cov11_ppt_mat(1, 0) = point_free_params_->sigma_cov11_ppt(1); 526 | sigma_cov11_ppt_mat(1, 1) = point_free_params_->sigma_cov11_ppt(3); 527 | sigma_cov11_ppt_mat(1, 2) = point_free_params_->sigma_cov11_ppt(4); 528 | sigma_cov11_ppt_mat(2, 0) = point_free_params_->sigma_cov11_ppt(2); 529 | sigma_cov11_ppt_mat(2, 1) = point_free_params_->sigma_cov11_ppt(4); 530 | sigma_cov11_ppt_mat(2, 2) = point_free_params_->sigma_cov11_ppt(5); 531 | 532 | Eigen::Matrix3d sigma_cov12_ppt_mat; 533 | sigma_cov12_ppt_mat(0, 0) = point_free_params_->sigma_cov12_ppt(0); 534 | sigma_cov12_ppt_mat(0, 1) = point_free_params_->sigma_cov12_ppt(1); 535 | sigma_cov12_ppt_mat(0, 2) = point_free_params_->sigma_cov12_ppt(2); 536 | sigma_cov12_ppt_mat(1, 0) = point_free_params_->sigma_cov12_ppt(1); 537 | sigma_cov12_ppt_mat(1, 1) = point_free_params_->sigma_cov12_ppt(3); 538 | sigma_cov12_ppt_mat(1, 2) = point_free_params_->sigma_cov12_ppt(4); 539 | sigma_cov12_ppt_mat(2, 0) = point_free_params_->sigma_cov12_ppt(2); 540 | sigma_cov12_ppt_mat(2, 1) = point_free_params_->sigma_cov12_ppt(4); 541 | sigma_cov12_ppt_mat(2, 2) = point_free_params_->sigma_cov12_ppt(5); 542 | 543 | Eigen::Matrix3d sigma_cov13_ppt_mat; 544 | sigma_cov13_ppt_mat(0, 0) = point_free_params_->sigma_cov13_ppt(0); 545 | sigma_cov13_ppt_mat(0, 1) = point_free_params_->sigma_cov13_ppt(1); 546 | sigma_cov13_ppt_mat(0, 2) = point_free_params_->sigma_cov13_ppt(2); 547 | sigma_cov13_ppt_mat(1, 0) = point_free_params_->sigma_cov13_ppt(1); 548 | sigma_cov13_ppt_mat(1, 1) = point_free_params_->sigma_cov13_ppt(3); 549 | sigma_cov13_ppt_mat(1, 2) = point_free_params_->sigma_cov13_ppt(4); 550 | sigma_cov13_ppt_mat(2, 0) = point_free_params_->sigma_cov13_ppt(2); 551 | sigma_cov13_ppt_mat(2, 1) = point_free_params_->sigma_cov13_ppt(4); 552 | sigma_cov13_ppt_mat(2, 2) = point_free_params_->sigma_cov13_ppt(5); 553 | 554 | Eigen::Matrix3d sigma_cov22_ppt_mat; 555 | sigma_cov22_ppt_mat(0, 0) = point_free_params_->sigma_cov22_ppt(0); 556 | sigma_cov22_ppt_mat(0, 1) = point_free_params_->sigma_cov22_ppt(1); 557 | sigma_cov22_ppt_mat(0, 2) = point_free_params_->sigma_cov22_ppt(2); 558 | sigma_cov22_ppt_mat(1, 0) = point_free_params_->sigma_cov22_ppt(1); 559 | sigma_cov22_ppt_mat(1, 1) = point_free_params_->sigma_cov22_ppt(3); 560 | sigma_cov22_ppt_mat(1, 2) = point_free_params_->sigma_cov22_ppt(4); 561 | sigma_cov22_ppt_mat(2, 0) = point_free_params_->sigma_cov22_ppt(2); 562 | sigma_cov22_ppt_mat(2, 1) = point_free_params_->sigma_cov22_ppt(4); 563 | sigma_cov22_ppt_mat(2, 2) = point_free_params_->sigma_cov22_ppt(5); 564 | 565 | Eigen::Matrix3d sigma_cov23_ppt_mat; 566 | sigma_cov23_ppt_mat(0, 0) = point_free_params_->sigma_cov23_ppt(0); 567 | sigma_cov23_ppt_mat(0, 1) = point_free_params_->sigma_cov23_ppt(1); 568 | sigma_cov23_ppt_mat(0, 2) = point_free_params_->sigma_cov23_ppt(2); 569 | sigma_cov23_ppt_mat(1, 0) = point_free_params_->sigma_cov23_ppt(1); 570 | sigma_cov23_ppt_mat(1, 1) = point_free_params_->sigma_cov23_ppt(3); 571 | sigma_cov23_ppt_mat(1, 2) = point_free_params_->sigma_cov23_ppt(4); 572 | sigma_cov23_ppt_mat(2, 0) = point_free_params_->sigma_cov23_ppt(2); 573 | sigma_cov23_ppt_mat(2, 1) = point_free_params_->sigma_cov23_ppt(4); 574 | sigma_cov23_ppt_mat(2, 2) = point_free_params_->sigma_cov23_ppt(5); 575 | 576 | Eigen::Matrix3d sigma_cov33_ppt_mat; 577 | sigma_cov33_ppt_mat(0, 0) = point_free_params_->sigma_cov33_ppt(0); 578 | sigma_cov33_ppt_mat(0, 1) = point_free_params_->sigma_cov33_ppt(1); 579 | sigma_cov33_ppt_mat(0, 2) = point_free_params_->sigma_cov33_ppt(2); 580 | sigma_cov33_ppt_mat(1, 0) = point_free_params_->sigma_cov33_ppt(1); 581 | sigma_cov33_ppt_mat(1, 1) = point_free_params_->sigma_cov33_ppt(3); 582 | sigma_cov33_ppt_mat(1, 2) = point_free_params_->sigma_cov33_ppt(4); 583 | sigma_cov33_ppt_mat(2, 0) = point_free_params_->sigma_cov33_ppt(2); 584 | sigma_cov33_ppt_mat(2, 1) = point_free_params_->sigma_cov33_ppt(4); 585 | sigma_cov33_ppt_mat(2, 2) = point_free_params_->sigma_cov33_ppt(5); 586 | 587 | Eigen::Matrix3d sigma_cov_mat; 588 | sigma_cov_mat(0, 0) = point_free_params_->sigma_cov(0); 589 | sigma_cov_mat(0, 1) = point_free_params_->sigma_cov(1); 590 | sigma_cov_mat(0, 2) = point_free_params_->sigma_cov(2); 591 | sigma_cov_mat(1, 0) = point_free_params_->sigma_cov(1); 592 | sigma_cov_mat(1, 1) = point_free_params_->sigma_cov(3); 593 | sigma_cov_mat(1, 2) = point_free_params_->sigma_cov(4); 594 | sigma_cov_mat(2, 0) = point_free_params_->sigma_cov(2); 595 | sigma_cov_mat(2, 1) = point_free_params_->sigma_cov(4); 596 | sigma_cov_mat(2, 2) = point_free_params_->sigma_cov(5); 597 | 598 | // Calculate plane_cov.block<3, 3>(0, 0) 599 | for (int i = 0; i < 3; i++) { 600 | if (i != min_eval_idx) { 601 | for (int j = i; j < 3; j++) { 602 | if (j != min_eval_idx) { 603 | const auto& B1 = B[i]; 604 | const auto& B2 = B[j].transpose(); 605 | 606 | double sigma_pt_B1_cov_B2t_p = (sigma_cov11_ppt_mat * B1 * e1 * e1.transpose() * B2) 607 | .trace() + 608 | (sigma_cov12_ppt_mat * B1 * e1 * e2.transpose() * B2) 609 | .trace() + 610 | (sigma_cov13_ppt_mat * B1 * e1 * e3.transpose() * B2) 611 | .trace() + 612 | (sigma_cov12_ppt_mat * B1 * e2 * e1.transpose() * B2) 613 | .trace() + 614 | (sigma_cov22_ppt_mat * B1 * e2 * e2.transpose() * B2) 615 | .trace() + 616 | (sigma_cov23_ppt_mat * B1 * e2 * e3.transpose() * B2) 617 | .trace() + 618 | (sigma_cov13_ppt_mat * B1 * e3 * e1.transpose() * B2) 619 | .trace() + 620 | (sigma_cov23_ppt_mat * B1 * e3 * e2.transpose() * B2) 621 | .trace() + 622 | (sigma_cov33_ppt_mat * B1 * e3 * e3.transpose() * B2) 623 | .trace(); 624 | 625 | double e1t_B2_q = e1.transpose() * B2 * center; 626 | double e2t_B2_q = e2.transpose() * B2 * center; 627 | double e3t_B2_q = e3.transpose() * B2 * center; 628 | double e1t_B1_q = e1.transpose() * B1 * center; 629 | double e2t_B1_q = e2.transpose() * B1 * center; 630 | double e3t_B1_q = e3.transpose() * B1 * center; 631 | double sigma_pt_B1_cov_B2t_q = e1t_B2_q * 632 | (point_free_params_->sigma_cov_e1_pt * B1).trace() + 633 | e2t_B2_q * 634 | (point_free_params_->sigma_cov_e2_pt * B1).trace() + 635 | e3t_B2_q * 636 | (point_free_params_->sigma_cov_e3_pt * B1).trace(); 637 | 638 | double sigma_qt_B1_cov_B2t_p = e1t_B1_q * 639 | (point_free_params_->sigma_cov_e1_pt * B2).trace() + 640 | e2t_B1_q * 641 | (point_free_params_->sigma_cov_e2_pt * B2).trace() + 642 | e3t_B1_q * 643 | (point_free_params_->sigma_cov_e3_pt * B2).trace(); 644 | 645 | double sigma_qt_B1_cov_B2t_q = center.transpose() * B1 * sigma_cov_mat * B2 * center; 646 | 647 | plane_cov(i, j) = sigma_pt_B1_cov_B2t_p - sigma_pt_B1_cov_B2t_q - 648 | sigma_qt_B1_cov_B2t_p + sigma_qt_B1_cov_B2t_q; 649 | } 650 | } 651 | } 652 | } 653 | for (size_t i = 0; i < 3; i++) { 654 | for (size_t j = i + 1; j < 3; j++) { 655 | plane_cov(j, i) = plane_cov(i, j); 656 | } 657 | } 658 | plane_cov.block<3, 3>(0, 0) = J_n_pw * plane_cov.block<3, 3>(0, 0).eval() * J_n_pw.transpose(); 659 | 660 | // Calculate plane_cov.block<3, 3>(0, 3) and plane_cov.block<3, 3>(3, 0) 661 | for (int i = 0; i < 3; i++) { 662 | if (i != min_eval_idx) { 663 | const auto& Bk = B[i]; 664 | 665 | Eigen::Vector3d pt_Bk_cov; 666 | Eigen::Vector3d qt_Bk_cov; 667 | 668 | pt_Bk_cov(0) = (point_free_params_->sigma_cov_e1_pt * Bk).trace(); 669 | pt_Bk_cov(1) = (point_free_params_->sigma_cov_e2_pt * Bk).trace(); 670 | pt_Bk_cov(2) = (point_free_params_->sigma_cov_e3_pt * Bk).trace(); 671 | 672 | qt_Bk_cov = center.transpose() * Bk * sigma_cov_mat; 673 | plane_cov.block<1, 3>(i, 3) = pt_Bk_cov - qt_Bk_cov; 674 | } 675 | } 676 | plane_cov.block<3, 3>(0, 3) = J_n_pw * plane_cov.block<3, 3>(0, 3).eval() / 677 | point_free_params_->point_nums; 678 | plane_cov.block<3, 3>(3, 0) = plane_cov.block<3, 3>(0, 3).transpose(); 679 | 680 | // Calculate plane_cov.block<3, 3>(3, 3) 681 | plane_cov.block<3, 3>(3, 3) = sigma_cov_mat / 682 | (point_free_params_->point_nums * point_free_params_->point_nums); 683 | } 684 | 685 | // check is plane , calc plane parameters including plane covariance 686 | void init_plane(const std::vector& points, Plane* plane) { 687 | plane->plane_cov = Eigen::Matrix::Zero(); 688 | plane->covariance = Eigen::Matrix3d::Zero(); 689 | plane->center = Eigen::Vector3d::Zero(); 690 | plane->normal = Eigen::Vector3d::Zero(); 691 | plane->radius = 0; 692 | 693 | if (!init_octo_) { 694 | plane->points_size = points.size(); 695 | for (auto pv : points) { 696 | plane->covariance += pv.point * pv.point.transpose(); 697 | plane->center += pv.point; 698 | } 699 | plane->center = plane->center / plane->points_size; 700 | plane->covariance = plane->covariance / plane->points_size - 701 | plane->center * plane->center.transpose(); 702 | } else { 703 | Eigen::Matrix3d sigma_p_pt_mat; 704 | sigma_p_pt_mat(0, 0) = point_free_params_->sigma_p_pt(0); 705 | sigma_p_pt_mat(0, 1) = point_free_params_->sigma_p_pt(1); 706 | sigma_p_pt_mat(0, 2) = point_free_params_->sigma_p_pt(2); 707 | sigma_p_pt_mat(1, 0) = point_free_params_->sigma_p_pt(1); 708 | sigma_p_pt_mat(1, 1) = point_free_params_->sigma_p_pt(3); 709 | sigma_p_pt_mat(1, 2) = point_free_params_->sigma_p_pt(4); 710 | sigma_p_pt_mat(2, 0) = point_free_params_->sigma_p_pt(2); 711 | sigma_p_pt_mat(2, 1) = point_free_params_->sigma_p_pt(4); 712 | sigma_p_pt_mat(2, 2) = point_free_params_->sigma_p_pt(5); 713 | plane->points_size = point_free_params_->point_nums; 714 | plane->center = point_free_params_->sigma_p / plane->points_size; 715 | plane->covariance = sigma_p_pt_mat / plane->points_size - 716 | plane->center * plane->center.transpose(); 717 | } 718 | 719 | Eigen::EigenSolver es(plane->covariance); 720 | Eigen::Matrix3cd evecs = es.eigenvectors(); 721 | Eigen::Vector3cd evals = es.eigenvalues(); 722 | Eigen::Vector3d evalsReal; 723 | evalsReal = evals.real(); 724 | Eigen::Matrix3d::Index evalsMin, evalsMax; 725 | evalsReal.rowwise().sum().minCoeff(&evalsMin); 726 | evalsReal.rowwise().sum().maxCoeff(&evalsMax); 727 | int evalsMid = 3 - evalsMin - evalsMax; 728 | Eigen::Vector3d evecMin = evecs.real().col(evalsMin); 729 | Eigen::Vector3d evecMid = evecs.real().col(evalsMid); 730 | Eigen::Vector3d evecMax = evecs.real().col(evalsMax); 731 | // plane covariance calculation 732 | Eigen::Matrix3d J_Q; 733 | J_Q << 1.0 / plane->points_size, 0, 0, 0, 1.0 / plane->points_size, 0, 0, 0, 734 | 1.0 / plane->points_size; 735 | 736 | if (evalsReal(evalsMin) < planer_threshold_) { 737 | if (!init_octo_) { 738 | point_free_params_ = new PointFreeParams(); 739 | point_free_params_->Initialize(); 740 | for (const auto& pv : temp_points_) { 741 | UpdatePointFreeParams(pv); 742 | } 743 | } 744 | std::vector B(3); 745 | // Jacobian of the normal of plane w.r.t the world point 746 | auto J_n_pw = evecs.real(); 747 | std::vector F_aux(3); 748 | for (int m = 0; m < 3; m++) { 749 | if (m != (int) evalsMin) { 750 | B[m] = (evecs.real().col(m) * evecs.real().col(evalsMin).transpose() + 751 | evecs.real().col(evalsMin) * evecs.real().col(m).transpose()) / 752 | (plane->points_size * (evalsReal[evalsMin] - evalsReal[m])); 753 | } else { 754 | B[m].setZero(); 755 | } 756 | } 757 | CalculatePointFreeVoxelPlaneCovariance(plane->plane_cov, 758 | plane->center, 759 | B, 760 | J_n_pw, 761 | (int) evalsMin); 762 | 763 | plane->normal << evecs.real()(0, evalsMin), evecs.real()(1, evalsMin), 764 | evecs.real()(2, evalsMin); 765 | plane->y_normal << evecs.real()(0, evalsMid), evecs.real()(1, evalsMid), 766 | evecs.real()(2, evalsMid); 767 | plane->x_normal << evecs.real()(0, evalsMax), evecs.real()(1, evalsMax), 768 | evecs.real()(2, evalsMax); 769 | plane->min_eigen_value = evalsReal(evalsMin); 770 | plane->mid_eigen_value = evalsReal(evalsMid); 771 | plane->max_eigen_value = evalsReal(evalsMax); 772 | plane->radius = sqrt(evalsReal(evalsMax)); 773 | plane->d = -(plane->normal(0) * plane->center(0) + plane->normal(1) * plane->center(1) + 774 | plane->normal(2) * plane->center(2)); 775 | 776 | plane->is_plane = true; 777 | if (plane->last_update_points_size == 0) { 778 | plane->last_update_points_size = plane->points_size; 779 | plane->is_update = true; 780 | } else if (plane->points_size - plane->last_update_points_size > 100) { 781 | plane->last_update_points_size = plane->points_size; 782 | plane->is_update = true; 783 | } 784 | 785 | if (!plane->is_init) { 786 | plane->id = plane_id; 787 | plane_id++; 788 | plane->is_init = true; 789 | } 790 | } else { 791 | if (!plane->is_init) { 792 | plane->id = plane_id; 793 | plane_id++; 794 | plane->is_init = true; 795 | } 796 | if (plane->last_update_points_size == 0) { 797 | plane->last_update_points_size = plane->points_size; 798 | plane->is_update = true; 799 | } else if (plane->points_size - plane->last_update_points_size > 100) { 800 | plane->last_update_points_size = plane->points_size; 801 | plane->is_update = true; 802 | } 803 | plane->is_plane = false; 804 | plane->normal << evecs.real()(0, evalsMin), evecs.real()(1, evalsMin), 805 | evecs.real()(2, evalsMin); 806 | plane->y_normal << evecs.real()(0, evalsMid), evecs.real()(1, evalsMid), 807 | evecs.real()(2, evalsMid); 808 | plane->x_normal << evecs.real()(0, evalsMax), evecs.real()(1, evalsMax), 809 | evecs.real()(2, evalsMax); 810 | plane->min_eigen_value = evalsReal(evalsMin); 811 | plane->mid_eigen_value = evalsReal(evalsMid); 812 | plane->max_eigen_value = evalsReal(evalsMax); 813 | plane->radius = sqrt(evalsReal(evalsMax)); 814 | plane->d = -(plane->normal(0) * plane->center(0) + plane->normal(1) * plane->center(1) + 815 | plane->normal(2) * plane->center(2)); 816 | } 817 | } 818 | 819 | void UpdatePlaneCov() { 820 | plane_ptr_->plane_cov = Eigen::Matrix::Zero(); 821 | plane_ptr_->covariance = Eigen::Matrix3d::Zero(); 822 | plane_ptr_->center = Eigen::Vector3d::Zero(); 823 | plane_ptr_->normal = Eigen::Vector3d::Zero(); 824 | plane_ptr_->radius = 0; 825 | 826 | Eigen::Matrix3d sigma_p_pt_mat; 827 | sigma_p_pt_mat(0, 0) = point_free_params_->sigma_p_pt(0); 828 | sigma_p_pt_mat(0, 1) = point_free_params_->sigma_p_pt(1); 829 | sigma_p_pt_mat(0, 2) = point_free_params_->sigma_p_pt(2); 830 | sigma_p_pt_mat(1, 0) = point_free_params_->sigma_p_pt(1); 831 | sigma_p_pt_mat(1, 1) = point_free_params_->sigma_p_pt(3); 832 | sigma_p_pt_mat(1, 2) = point_free_params_->sigma_p_pt(4); 833 | sigma_p_pt_mat(2, 0) = point_free_params_->sigma_p_pt(2); 834 | sigma_p_pt_mat(2, 1) = point_free_params_->sigma_p_pt(4); 835 | sigma_p_pt_mat(2, 2) = point_free_params_->sigma_p_pt(5); 836 | plane_ptr_->points_size = point_free_params_->point_nums; 837 | plane_ptr_->center = point_free_params_->sigma_p / plane_ptr_->points_size; 838 | plane_ptr_->covariance = sigma_p_pt_mat / plane_ptr_->points_size - 839 | plane_ptr_->center * plane_ptr_->center.transpose(); 840 | 841 | Eigen::EigenSolver es(plane_ptr_->covariance); 842 | Eigen::Matrix3cd evecs = es.eigenvectors(); 843 | Eigen::Vector3cd evals = es.eigenvalues(); 844 | Eigen::Vector3d evalsReal; 845 | evalsReal = evals.real(); 846 | Eigen::Matrix3d::Index evalsMin, evalsMax; 847 | evalsReal.rowwise().sum().minCoeff(&evalsMin); 848 | evalsReal.rowwise().sum().maxCoeff(&evalsMax); 849 | int evalsMid = 3 - evalsMin - evalsMax; 850 | Eigen::Vector3d evecMin = evecs.real().col(evalsMin); 851 | Eigen::Vector3d evecMid = evecs.real().col(evalsMid); 852 | Eigen::Vector3d evecMax = evecs.real().col(evalsMax); 853 | // plane covariance calculation 854 | Eigen::Matrix3d J_Q; 855 | J_Q << 1.0 / plane_ptr_->points_size, 0, 0, 0, 1.0 / plane_ptr_->points_size, 0, 0, 0, 856 | 1.0 / plane_ptr_->points_size; 857 | 858 | std::vector B(3); 859 | // Jacobian of the normal of plane w.r.t the world point 860 | auto J_n_pw = evecs.real(); 861 | std::vector F_aux(3); 862 | for (int m = 0; m < 3; m++) { 863 | if (m != (int) evalsMin) { 864 | B[m] = (evecs.real().col(m) * evecs.real().col(evalsMin).transpose() + 865 | evecs.real().col(evalsMin) * evecs.real().col(m).transpose()) / 866 | (plane_ptr_->points_size * (evalsReal[evalsMin] - evalsReal[m])); 867 | } else { 868 | B[m].setZero(); 869 | } 870 | } 871 | CalculatePointFreeVoxelPlaneCovariance(plane_ptr_->plane_cov, 872 | plane_ptr_->center, 873 | B, 874 | J_n_pw, 875 | (int) evalsMin); 876 | 877 | plane_ptr_->normal << evecs.real()(0, evalsMin), evecs.real()(1, evalsMin), 878 | evecs.real()(2, evalsMin); 879 | plane_ptr_->y_normal << evecs.real()(0, evalsMid), evecs.real()(1, evalsMid), 880 | evecs.real()(2, evalsMid); 881 | plane_ptr_->x_normal << evecs.real()(0, evalsMax), evecs.real()(1, evalsMax), 882 | evecs.real()(2, evalsMax); 883 | plane_ptr_->min_eigen_value = evalsReal(evalsMin); 884 | plane_ptr_->mid_eigen_value = evalsReal(evalsMid); 885 | plane_ptr_->max_eigen_value = evalsReal(evalsMax); 886 | plane_ptr_->radius = sqrt(evalsReal(evalsMax)); 887 | plane_ptr_->d = -(plane_ptr_->normal(0) * plane_ptr_->center(0) + 888 | plane_ptr_->normal(1) * plane_ptr_->center(1) + 889 | plane_ptr_->normal(2) * plane_ptr_->center(2)); 890 | 891 | plane_ptr_->is_plane = true; 892 | if (plane_ptr_->last_update_points_size == 0) { 893 | plane_ptr_->last_update_points_size = plane_ptr_->points_size; 894 | plane_ptr_->is_update = true; 895 | } else if (plane_ptr_->points_size - plane_ptr_->last_update_points_size > 100) { 896 | plane_ptr_->last_update_points_size = plane_ptr_->points_size; 897 | plane_ptr_->is_update = true; 898 | } 899 | } 900 | 901 | void init_octo_tree(std::vector& leaf_path, const VOXEL_LOC& voxel_loc) { 902 | if (temp_points_.size() > max_plane_update_threshold_) { 903 | init_plane(temp_points_, plane_ptr_); 904 | if (plane_ptr_->is_plane == true) { 905 | octo_state_ = 0; 906 | } else { 907 | if (layer_ < max_layer_) { 908 | octo_state_ = 1; 909 | cut_octo_tree(leaf_path, voxel_loc); 910 | } else { 911 | point_free_params_ = new PointFreeParams(); 912 | point_free_params_->Initialize(); 913 | for (const auto& pv : temp_points_) { 914 | UpdatePointFreeParams(pv); 915 | } 916 | } 917 | } 918 | std::vector().swap(temp_points_); 919 | init_octo_ = true; 920 | } 921 | } 922 | 923 | void cut_octo_tree(std::vector& leaf_path, const VOXEL_LOC& voxel_loc) { 924 | if (layer_ == max_layer_) { 925 | octo_state_ = 0; 926 | return; 927 | } 928 | for (size_t i = 0; i < temp_points_.size(); i++) { 929 | int xyz[3] = {0, 0, 0}; 930 | if (temp_points_[i].point[0] > voxel_center_[0]) { 931 | xyz[0] = 1; 932 | } 933 | if (temp_points_[i].point[1] > voxel_center_[1]) { 934 | xyz[1] = 1; 935 | } 936 | if (temp_points_[i].point[2] > voxel_center_[2]) { 937 | xyz[2] = 1; 938 | } 939 | int leafnum = 4 * xyz[0] + 2 * xyz[1] + xyz[2]; 940 | if (leaves_[leafnum] == nullptr) { 941 | leaves_[leafnum] = new OctoTree(max_layer_, 942 | layer_ + 1, 943 | layer_point_size_, 944 | planer_threshold_); 945 | voxel_nums++; 946 | leaves_[leafnum]->voxel_center_[0] = voxel_center_[0] + (2 * xyz[0] - 1) * quater_length_; 947 | leaves_[leafnum]->voxel_center_[1] = voxel_center_[1] + (2 * xyz[1] - 1) * quater_length_; 948 | leaves_[leafnum]->voxel_center_[2] = voxel_center_[2] + (2 * xyz[2] - 1) * quater_length_; 949 | leaves_[leafnum]->quater_length_ = quater_length_ / 2; 950 | leaf_path.push_back(leafnum); 951 | std::bitset<32> leaf_id; 952 | leaf_id.reset(); 953 | for (size_t i = 0; i < leaf_path.size(); i++) { 954 | size_t bit_idx = 4 * i + 1; 955 | if (i == leaf_path.size() - 1) { 956 | leaf_id[bit_idx] = 1; 957 | } 958 | size_t leaf_idx = leaf_path[i]; 959 | leaf_id[bit_idx + 1] = leaf_idx & 1; 960 | leaf_id[bit_idx + 2] = (leaf_idx >> 1) & 1; 961 | leaf_id[bit_idx + 3] = (leaf_idx >> 2) & 1; 962 | } 963 | leaf_path.pop_back(); 964 | } 965 | leaves_[leafnum]->temp_points_.push_back(temp_points_[i]); 966 | } 967 | for (size_t leafnum = 0; leafnum < 8; ++leafnum) { 968 | if (leaves_[leafnum]) { 969 | if (leaves_[leafnum]->temp_points_.size() > leaves_[leafnum]->max_plane_update_threshold_) { 970 | leaf_path.push_back(leafnum); 971 | leaves_[leafnum]->init_octo_tree(leaf_path, voxel_loc); 972 | leaf_path.pop_back(); 973 | } 974 | } 975 | } 976 | } 977 | 978 | void UpdateOctoTree(bool& plane_is_update, 979 | std::vector& plane_leaf_path, 980 | bool& node_need_split, 981 | const pointWithCov& pv, 982 | std::vector& leaf_path, 983 | const VOXEL_LOC& voxel_loc, 984 | uint32_t frame_num) { 985 | last_update_frame_ = frame_num; 986 | if (!init_octo_) { 987 | point_cluster_.Push(pv.point); 988 | temp_points_.push_back(pv); 989 | if (temp_points_.size() > max_plane_update_threshold_) { 990 | init_octo_tree(leaf_path, voxel_loc); 991 | } 992 | } else { 993 | if (plane_ptr_->is_plane) { 994 | point_cluster_.Push(pv.point); 995 | if (plane_ptr_->merge_plane_id >= 0) { 996 | node_need_split = UpdateMergedPlanePointFreeParams(pv); 997 | UpdatePlaneCov(); 998 | } else { 999 | UpdatePointFreeParams(pv); 1000 | init_plane(temp_points_, plane_ptr_); 1001 | if (plane_ptr_->is_plane) { 1002 | plane_is_update = true; 1003 | } 1004 | } 1005 | } else { 1006 | if (layer_ < max_layer_) { 1007 | if (temp_points_.size() != 0) { 1008 | std::vector().swap(temp_points_); 1009 | } 1010 | if (new_points_.size() != 0) { 1011 | std::vector().swap(new_points_); 1012 | } 1013 | int xyz[3] = {0, 0, 0}; 1014 | if (pv.point[0] > voxel_center_[0]) { 1015 | xyz[0] = 1; 1016 | } 1017 | if (pv.point[1] > voxel_center_[1]) { 1018 | xyz[1] = 1; 1019 | } 1020 | if (pv.point[2] > voxel_center_[2]) { 1021 | xyz[2] = 1; 1022 | } 1023 | int leafnum = 4 * xyz[0] + 2 * xyz[1] + xyz[2]; 1024 | 1025 | leaf_path.push_back(leafnum); 1026 | if (leaves_[leafnum] != nullptr) { 1027 | plane_leaf_path.push_back(leafnum); 1028 | leaves_[leafnum]->UpdateOctoTree(plane_is_update, 1029 | plane_leaf_path, 1030 | node_need_split, 1031 | pv, 1032 | leaf_path, 1033 | voxel_loc, 1034 | frame_num); 1035 | } else { 1036 | leaves_[leafnum] = new OctoTree(max_layer_, 1037 | layer_ + 1, 1038 | layer_point_size_, 1039 | planer_threshold_); 1040 | voxel_nums++; 1041 | leaves_[leafnum]->layer_point_size_ = layer_point_size_; 1042 | leaves_[leafnum]->voxel_center_[0] = voxel_center_[0] + 1043 | (2 * xyz[0] - 1) * quater_length_; 1044 | leaves_[leafnum]->voxel_center_[1] = voxel_center_[1] + 1045 | (2 * xyz[1] - 1) * quater_length_; 1046 | leaves_[leafnum]->voxel_center_[2] = voxel_center_[2] + 1047 | (2 * xyz[2] - 1) * quater_length_; 1048 | leaves_[leafnum]->quater_length_ = quater_length_ / 2; 1049 | 1050 | std::bitset<32> leaf_id; 1051 | leaf_id.reset(); 1052 | for (size_t i = 0; i < leaf_path.size(); i++) { 1053 | size_t bit_idx = 4 * i + 1; 1054 | if (i == leaf_path.size() - 1) { 1055 | leaf_id[bit_idx] = 1; 1056 | } 1057 | size_t leaf_idx = leaf_path[i]; 1058 | leaf_id[bit_idx + 1] = leaf_idx & 1; 1059 | leaf_id[bit_idx + 2] = (leaf_idx >> 1) & 1; 1060 | leaf_id[bit_idx + 3] = (leaf_idx >> 2) & 1; 1061 | } 1062 | 1063 | plane_leaf_path.push_back(leafnum); 1064 | leaves_[leafnum]->UpdateOctoTree(plane_is_update, 1065 | plane_leaf_path, 1066 | node_need_split, 1067 | pv, 1068 | leaf_path, 1069 | voxel_loc, 1070 | frame_num); 1071 | } 1072 | leaf_path.pop_back(); 1073 | } else { 1074 | point_cluster_.Push(pv.point); 1075 | UpdatePointFreeParams(pv); 1076 | init_plane(temp_points_, plane_ptr_); 1077 | } 1078 | } 1079 | } 1080 | } 1081 | 1082 | void MergeNode(OctoTree* merged_node, int64_t leaf_num, bool need_delete) { 1083 | int64_t leaf_layer_num = leaf_num / 10; 1084 | int64_t leaf_idx = leaf_num % 10 - 1; 1085 | 1086 | if (leaf_layer_num == 0) { 1087 | if (need_delete) { 1088 | merged_voxel_nums++; 1089 | merged_voxel_nums += leaves_[leaf_idx]->GetLeafNums(); 1090 | delete leaves_[leaf_idx]; 1091 | } 1092 | leaves_[leaf_idx] = merged_node; 1093 | return; 1094 | } else { 1095 | leaves_[leaf_idx]->MergeNode(merged_node, leaf_layer_num, need_delete); 1096 | } 1097 | } 1098 | 1099 | void SplitMergedNode(std::vector& leaf_path, 1100 | const VOXEL_LOC& voxel_loc, 1101 | const pointWithCov& pv, 1102 | uint32_t frame_num, 1103 | int64_t leaf_num, 1104 | int layer) { 1105 | int64_t leaf_layer_num = leaf_num / 10; 1106 | int64_t leaf_idx = leaf_num % 10 - 1; 1107 | 1108 | int xyz[3] = {0, 0, 0}; 1109 | if (pv.point[0] > voxel_center_[0]) { 1110 | xyz[0] = 1; 1111 | } 1112 | if (pv.point[1] > voxel_center_[1]) { 1113 | xyz[1] = 1; 1114 | } 1115 | if (pv.point[2] > voxel_center_[2]) { 1116 | xyz[2] = 1; 1117 | } 1118 | int leafnum = 4 * xyz[0] + 2 * xyz[1] + xyz[2]; 1119 | 1120 | if (leaf_layer_num == 0) { 1121 | OctoTree* node = new OctoTree(leaves_[leaf_idx]->max_layer_, 1122 | layer, 1123 | leaves_[leaf_idx]->layer_point_size_, 1124 | leaves_[leaf_idx]->planer_threshold_); 1125 | voxel_nums++; 1126 | leaves_[leaf_idx] = node; 1127 | 1128 | leaves_[leaf_idx]->voxel_center_[0] = voxel_center_[0] + (2 * xyz[0] - 1) * quater_length_; 1129 | leaves_[leaf_idx]->voxel_center_[1] = voxel_center_[1] + (2 * xyz[1] - 1) * quater_length_; 1130 | leaves_[leaf_idx]->voxel_center_[2] = voxel_center_[2] + (2 * xyz[2] - 1) * quater_length_; 1131 | leaves_[leaf_idx]->quater_length_ = quater_length_ / 2; 1132 | 1133 | leaves_[leaf_idx]->last_update_frame_ = frame_num; 1134 | leaves_[leaf_idx]->temp_points_.push_back(pv); 1135 | leaves_[leaf_idx]->point_cluster_.Push(pv.point); 1136 | 1137 | leaf_path.push_back(leaf_idx); 1138 | std::bitset<32> leaf_id; 1139 | leaf_id.reset(); 1140 | for (size_t i = 0; i < leaf_path.size(); i++) { 1141 | size_t bit_idx = 4 * i + 1; 1142 | if (i == leaf_path.size() - 1) { 1143 | leaf_id[bit_idx] = 1; 1144 | } 1145 | size_t leaf_idx = leaf_path[i]; 1146 | leaf_id[bit_idx + 1] = leaf_idx & 1; 1147 | leaf_id[bit_idx + 2] = (leaf_idx >> 1) & 1; 1148 | leaf_id[bit_idx + 3] = (leaf_idx >> 2) & 1; 1149 | } 1150 | return; 1151 | } else { 1152 | leaf_path.push_back(leaf_idx); 1153 | leaves_[leaf_idx] 1154 | ->SplitMergedNode(leaf_path, voxel_loc, pv, frame_num, leaf_layer_num, layer + 1); 1155 | } 1156 | } 1157 | }; 1158 | 1159 | void mapJet(double v, double vmin, double vmax, uint8_t& r, uint8_t& g, uint8_t& b) { 1160 | r = 255; 1161 | g = 255; 1162 | b = 255; 1163 | 1164 | if (v < vmin) { 1165 | v = vmin; 1166 | } 1167 | 1168 | if (v > vmax) { 1169 | v = vmax; 1170 | } 1171 | 1172 | double dr, dg, db; 1173 | 1174 | if (v < 0.1242) { 1175 | db = 0.504 + ((1. - 0.504) / 0.1242) * v; 1176 | dg = dr = 0.; 1177 | } else if (v < 0.3747) { 1178 | db = 1.; 1179 | dr = 0.; 1180 | dg = (v - 0.1242) * (1. / (0.3747 - 0.1242)); 1181 | } else if (v < 0.6253) { 1182 | db = (0.6253 - v) * (1. / (0.6253 - 0.3747)); 1183 | dg = 1.; 1184 | dr = (v - 0.3747) * (1. / (0.6253 - 0.3747)); 1185 | } else if (v < 0.8758) { 1186 | db = 0.; 1187 | dr = 1.; 1188 | dg = (0.8758 - v) * (1. / (0.8758 - 0.6253)); 1189 | } else { 1190 | db = 0.; 1191 | dg = 0.; 1192 | dr = 1. - (v - 0.8758) * ((1. - 0.504) / (1. - 0.8758)); 1193 | } 1194 | 1195 | r = (uint8_t)(255 * dr); 1196 | g = (uint8_t)(255 * dg); 1197 | b = (uint8_t)(255 * db); 1198 | } 1199 | 1200 | bool CheckIfMerge(std::pair& target_plane_coeff, 1201 | PointCluster& target_cluster, 1202 | const Plane* const candidate, 1203 | const PointCluster& candidate_cluster, 1204 | double merge_theta_thresh, 1205 | double merge_dist_thresh, 1206 | double merge_cov_min_eigen_val_thresh, 1207 | double merge_x_coord_diff_thresh, 1208 | double merge_y_coord_diff_thresh) { 1209 | double length = (target_plane_coeff.second - candidate->center).norm(); 1210 | double dist = length * (target_plane_coeff.second - candidate->center) 1211 | .normalized() 1212 | .dot(target_plane_coeff.first); 1213 | 1214 | const auto& candidate_normal = candidate->normal; 1215 | if (std::abs(target_plane_coeff.first.normalized().dot(candidate_normal.normalized())) < 1216 | std::cos(merge_theta_thresh)) { 1217 | return false; 1218 | } 1219 | 1220 | if (std::abs(dist) > merge_dist_thresh) { 1221 | return false; 1222 | } 1223 | 1224 | PointCluster plane_merged = target_cluster + candidate_cluster; 1225 | 1226 | Eigen::SelfAdjointEigenSolver solver(plane_merged.Cov()); 1227 | 1228 | if (solver.eigenvalues()[0] < merge_cov_min_eigen_val_thresh && solver.eigenvalues()[0] > 0) { 1229 | target_cluster = plane_merged; 1230 | 1231 | target_plane_coeff.first = solver.eigenvectors().col(0); 1232 | target_plane_coeff.second = target_cluster.v / target_cluster.N; 1233 | 1234 | return true; 1235 | } 1236 | 1237 | return false; 1238 | } 1239 | 1240 | OctoTree* GetVoxelPlaneNodeRecursive(OctoTree* current_node, int64_t leaf_num) { 1241 | int64_t leaf_layer_num = leaf_num / 10; 1242 | int64_t leaf_idx = leaf_num % 10 - 1; 1243 | 1244 | if (leaf_layer_num == 0) { 1245 | return current_node->leaves_[leaf_idx]; 1246 | } 1247 | 1248 | return GetVoxelPlaneNodeRecursive(current_node->leaves_[leaf_idx], leaf_layer_num); 1249 | } 1250 | 1251 | OctoTree* GetVoxelPlaneNode(std::unordered_map& feat_map, 1252 | const NodeLoc& node_loc) { 1253 | VOXEL_LOC voxel_loc(node_loc.x, node_loc.y, node_loc.z); 1254 | 1255 | if (node_loc.leaf_num == 0) { 1256 | return feat_map[voxel_loc]; 1257 | } else { 1258 | return GetVoxelPlaneNodeRecursive(feat_map[voxel_loc], node_loc.leaf_num); 1259 | } 1260 | } 1261 | 1262 | void MergeVoxelPlaneNode(OctoTree* root_node, 1263 | std::unordered_map& feat_map, 1264 | const NodeLoc& node_loc, 1265 | bool need_delete) { 1266 | VOXEL_LOC voxel_loc(node_loc.x, node_loc.y, node_loc.z); 1267 | 1268 | if (node_loc.leaf_num == 0) { 1269 | if (need_delete) { 1270 | merged_voxel_nums++; 1271 | merged_voxel_nums += feat_map[voxel_loc]->GetLeafNums(); 1272 | delete feat_map[voxel_loc]; 1273 | } 1274 | feat_map[voxel_loc] = root_node; 1275 | } else { 1276 | feat_map[voxel_loc]->MergeNode(root_node, node_loc.leaf_num, need_delete); 1277 | } 1278 | } 1279 | 1280 | void UpdatePlaneHashMap(std::unordered_map& feat_map, 1281 | std::unordered_map& plane_map, 1282 | std::unordered_map>& merged_table, 1283 | const VOXEL_LOC& voxel_loc, 1284 | const std::vector& leaf_path, 1285 | double merge_theta_thresh, 1286 | double merge_dist_thresh, 1287 | double merge_cov_min_eigen_val_thresh, 1288 | double merge_x_coord_diff_thresh, 1289 | double merge_y_coord_diff_thresh) { 1290 | std::unordered_map unique_node_table; 1291 | 1292 | NodeLoc node_loc(voxel_loc, leaf_path); 1293 | 1294 | const OctoTree* const current_node = GetVoxelPlaneNode(feat_map, node_loc); 1295 | 1296 | if (!current_node->plane_ptr_->is_plane) { 1297 | return; 1298 | } 1299 | 1300 | PlaneDesc plane_loc(current_node->plane_ptr_, 1301 | merge_x_coord_diff_thresh, 1302 | merge_y_coord_diff_thresh); 1303 | auto plane_loc_it = plane_map.find(plane_loc); 1304 | 1305 | if (plane_loc_it != plane_map.end()) { 1306 | auto& union_plane_bucket = plane_loc_it->second.plane_bucket; 1307 | 1308 | if (union_plane_bucket.count(node_loc) == 0) { 1309 | union_plane_bucket.insert(node_loc); 1310 | 1311 | if (union_plane_bucket.size() > 4) { 1312 | bool has_initialized = false; 1313 | { 1314 | for (const auto& plane_i : union_plane_bucket) { 1315 | if (has_initialized && plane_i == plane_loc_it->second.root_node_loc) { 1316 | continue; 1317 | } 1318 | 1319 | const OctoTree* const p1 = GetVoxelPlaneNode(feat_map, plane_i); 1320 | if (!p1 || !p1->plane_ptr_->is_plane) { 1321 | continue; 1322 | } 1323 | 1324 | if (!has_initialized) { 1325 | plane_loc_it->second.root_node_loc = plane_i; 1326 | has_initialized = true; 1327 | continue; 1328 | } 1329 | 1330 | const OctoTree* const p2 = GetVoxelPlaneNode(feat_map, 1331 | plane_loc_it->second.root_node_loc); 1332 | 1333 | if (!p2 || p1->point_cluster_.N > p2->point_cluster_.N) { 1334 | plane_loc_it->second.root_node_loc = plane_i; 1335 | } 1336 | } 1337 | } 1338 | 1339 | if (!has_initialized) { 1340 | return; 1341 | } 1342 | 1343 | const auto& root_node_loc = plane_loc_it->second.root_node_loc; 1344 | 1345 | OctoTree* root_plane_content = GetVoxelPlaneNode(feat_map, root_node_loc); 1346 | 1347 | if (!root_plane_content) { 1348 | return; 1349 | } 1350 | 1351 | PointCluster merged_plane = root_plane_content->point_cluster_; 1352 | std::pair merged_plane_coeff = std::make_pair( 1353 | root_plane_content->plane_ptr_->normal, 1354 | root_plane_content->plane_ptr_->center); 1355 | 1356 | for (auto vit = union_plane_bucket.begin(); vit != union_plane_bucket.end(); ++vit) { 1357 | const auto& node_loc_cur = *vit; 1358 | 1359 | if (node_loc_cur == root_node_loc) { 1360 | continue; 1361 | } 1362 | 1363 | int node_cur_merged_id = -1; 1364 | { 1365 | OctoTree* voxel_plane_cur = GetVoxelPlaneNode(feat_map, node_loc_cur); 1366 | if (!voxel_plane_cur || !voxel_plane_cur->plane_ptr_->is_plane || 1367 | voxel_plane_cur == root_plane_content) { 1368 | continue; 1369 | } 1370 | 1371 | if (!CheckIfMerge(merged_plane_coeff, 1372 | merged_plane, 1373 | voxel_plane_cur->plane_ptr_, 1374 | voxel_plane_cur->point_cluster_, 1375 | merge_theta_thresh, 1376 | merge_dist_thresh, 1377 | merge_cov_min_eigen_val_thresh, 1378 | merge_x_coord_diff_thresh, 1379 | merge_y_coord_diff_thresh)) { 1380 | continue; 1381 | } 1382 | 1383 | if (root_plane_content->plane_ptr_->merge_plane_id < 0) { 1384 | root_plane_content->plane_ptr_->merge_plane_id = merged_plane_idx; 1385 | merged_plane_idx++; 1386 | } 1387 | 1388 | node_cur_merged_id = voxel_plane_cur->plane_ptr_->merge_plane_id; 1389 | 1390 | root_plane_content->MergeVoxelPlane(voxel_plane_cur); 1391 | } 1392 | 1393 | merged_table[root_plane_content->plane_ptr_->merge_plane_id].insert(root_node_loc); 1394 | merged_table[root_plane_content->plane_ptr_->merge_plane_id].insert(node_loc_cur); 1395 | 1396 | if (node_cur_merged_id >= 0) { 1397 | auto& nodes_with_id = merged_table[node_cur_merged_id]; 1398 | for (const auto& node_loc : nodes_with_id) { 1399 | MergeVoxelPlaneNode(root_plane_content, feat_map, node_loc, node_loc == node_loc_cur); 1400 | 1401 | merged_table[root_plane_content->plane_ptr_->merge_plane_id].insert(node_loc); 1402 | } 1403 | std::unordered_set().swap(nodes_with_id); 1404 | merged_table.erase(node_cur_merged_id); 1405 | } else { 1406 | MergeVoxelPlaneNode(root_plane_content, feat_map, node_loc_cur, true); 1407 | } 1408 | } 1409 | 1410 | std::unordered_set().swap(union_plane_bucket); 1411 | plane_map.erase(plane_loc_it); 1412 | 1413 | PlaneDesc root_plane_loc(root_plane_content->plane_ptr_, 1414 | merge_x_coord_diff_thresh, 1415 | merge_y_coord_diff_thresh); 1416 | auto root_plane_loc_it = plane_map.find(root_plane_loc); 1417 | if (root_plane_loc_it != plane_map.end()) { 1418 | root_plane_loc_it->second.plane_bucket.insert(root_node_loc); 1419 | } else { 1420 | UnionPlane union_plane; 1421 | union_plane.plane_bucket.insert(root_node_loc); 1422 | union_plane.root_node_loc = root_node_loc; 1423 | plane_map[root_plane_loc] = union_plane; 1424 | } 1425 | } 1426 | } 1427 | } else { 1428 | UnionPlane union_plane; 1429 | union_plane.plane_bucket.insert(node_loc); 1430 | union_plane.root_node_loc = node_loc; 1431 | plane_map[plane_loc] = union_plane; 1432 | } 1433 | } 1434 | 1435 | void buildVoxelMap(bool enable_voxel_merging, 1436 | const std::vector& input_points, 1437 | const float voxel_size, 1438 | const int max_layer, 1439 | const std::vector& layer_point_size, 1440 | const float planer_threshold, 1441 | std::unordered_map& feat_map, 1442 | std::unordered_map& plane_map, 1443 | std::unordered_map>& merged_table, 1444 | double merge_theta_thresh, 1445 | double merge_dist_thresh, 1446 | double merge_cov_min_eigen_val_thresh, 1447 | double merge_x_coord_diff_thresh, 1448 | double merge_y_coord_diff_thresh) { 1449 | uint plsize = input_points.size(); 1450 | for (uint i = 0; i < plsize; i++) { 1451 | const pointWithCov& p_v = input_points[i]; 1452 | float loc_xyz[3]; 1453 | for (int j = 0; j < 3; j++) { 1454 | loc_xyz[j] = p_v.point[j] / voxel_size; 1455 | if (loc_xyz[j] < 0) { 1456 | loc_xyz[j] -= 1.0; 1457 | } 1458 | } 1459 | VOXEL_LOC position((int64_t) loc_xyz[0], (int64_t) loc_xyz[1], (int64_t) loc_xyz[2]); 1460 | auto iter = feat_map.find(position); 1461 | if (iter != feat_map.end()) { 1462 | feat_map[position]->temp_points_.push_back(p_v); 1463 | 1464 | feat_map[position]->point_cluster_.Push(p_v.point); 1465 | } else { 1466 | OctoTree* octo_tree = new OctoTree(max_layer, 0, layer_point_size, planer_threshold); 1467 | voxel_nums++; 1468 | feat_map[position] = octo_tree; 1469 | feat_map[position]->quater_length_ = voxel_size / 4; 1470 | feat_map[position]->voxel_center_[0] = (0.5 + position.x) * voxel_size; 1471 | feat_map[position]->voxel_center_[1] = (0.5 + position.y) * voxel_size; 1472 | feat_map[position]->voxel_center_[2] = (0.5 + position.z) * voxel_size; 1473 | feat_map[position]->temp_points_.push_back(p_v); 1474 | feat_map[position]->layer_point_size_ = layer_point_size; 1475 | std::bitset<32> leaf_id; 1476 | leaf_id.reset(); 1477 | leaf_id[0] = 1; 1478 | feat_map[position]->point_cluster_.Push(p_v.point); 1479 | } 1480 | } 1481 | for (auto iter = feat_map.begin(); iter != feat_map.end(); ++iter) { 1482 | std::vector leaf_path; 1483 | iter->second->init_octo_tree(leaf_path, iter->first); 1484 | 1485 | if (enable_voxel_merging && iter->second->plane_ptr_->is_plane) { 1486 | UpdatePlaneHashMap(feat_map, 1487 | plane_map, 1488 | merged_table, 1489 | iter->first, 1490 | std::vector(), 1491 | merge_theta_thresh, 1492 | merge_dist_thresh, 1493 | merge_cov_min_eigen_val_thresh, 1494 | merge_x_coord_diff_thresh, 1495 | merge_y_coord_diff_thresh); 1496 | } 1497 | } 1498 | } 1499 | 1500 | void updateVoxelMap(bool enable_voxel_merging, 1501 | const std::vector& input_points, 1502 | const float voxel_size, 1503 | const int max_layer, 1504 | const std::vector& layer_point_size, 1505 | const float planer_threshold, 1506 | std::unordered_map& feat_map, 1507 | std::unordered_map& plane_map, 1508 | std::unordered_map>& merged_table, 1509 | uint32_t frame_num, 1510 | double merge_theta_thresh, 1511 | double merge_dist_thresh, 1512 | double merge_cov_min_eigen_val_thresh, 1513 | double merge_x_coord_diff_thresh, 1514 | double merge_y_coord_diff_thresh) { 1515 | uint plsize = input_points.size(); 1516 | 1517 | for (uint i = 0; i < plsize; i++) { 1518 | // Check plane of voxel is update 1519 | bool plane_is_update = false; 1520 | // Node path in voxel 1521 | std::vector plane_node_loc; 1522 | 1523 | const pointWithCov p_v = input_points[i]; 1524 | float loc_xyz[3]; 1525 | for (int j = 0; j < 3; j++) { 1526 | loc_xyz[j] = p_v.point[j] / voxel_size; 1527 | if (loc_xyz[j] < 0) { 1528 | loc_xyz[j] -= 1.0; 1529 | } 1530 | } 1531 | VOXEL_LOC position((int64_t) loc_xyz[0], (int64_t) loc_xyz[1], (int64_t) loc_xyz[2]); 1532 | 1533 | bool node_need_split = false; 1534 | auto iter = feat_map.find(position); 1535 | if (iter != feat_map.end()) { 1536 | std::vector leaf_path; 1537 | feat_map[position]->UpdateOctoTree(plane_is_update, 1538 | plane_node_loc, 1539 | node_need_split, 1540 | p_v, 1541 | leaf_path, 1542 | position, 1543 | frame_num); 1544 | } else { 1545 | OctoTree* octo_tree = new OctoTree(max_layer, 0, layer_point_size, planer_threshold); 1546 | voxel_nums++; 1547 | feat_map[position] = octo_tree; 1548 | feat_map[position]->quater_length_ = voxel_size / 4; 1549 | feat_map[position]->voxel_center_[0] = (0.5 + position.x) * voxel_size; 1550 | feat_map[position]->voxel_center_[1] = (0.5 + position.y) * voxel_size; 1551 | feat_map[position]->voxel_center_[2] = (0.5 + position.z) * voxel_size; 1552 | std::bitset<32> leaf_id; 1553 | leaf_id.reset(); 1554 | leaf_id[0] = 1; 1555 | std::vector leaf_path; 1556 | feat_map[position]->UpdateOctoTree(plane_is_update, 1557 | plane_node_loc, 1558 | node_need_split, 1559 | p_v, 1560 | leaf_path, 1561 | position, 1562 | frame_num); 1563 | } 1564 | 1565 | if (enable_voxel_merging) { 1566 | if (plane_is_update) { 1567 | UpdatePlaneHashMap(feat_map, 1568 | plane_map, 1569 | merged_table, 1570 | position, 1571 | plane_node_loc, 1572 | merge_theta_thresh, 1573 | merge_dist_thresh, 1574 | merge_cov_min_eigen_val_thresh, 1575 | merge_x_coord_diff_thresh, 1576 | merge_y_coord_diff_thresh); 1577 | } 1578 | } 1579 | } 1580 | } 1581 | 1582 | void transformLidar(const StatesGroup& state, 1583 | const shared_ptr& p_imu, 1584 | const PointCloudXYZI::Ptr& input_cloud, 1585 | pcl::PointCloud::Ptr& trans_cloud) { 1586 | trans_cloud->clear(); 1587 | for (size_t i = 0; i < input_cloud->size(); i++) { 1588 | pcl::PointXYZINormal p_c = input_cloud->points[i]; 1589 | Eigen::Vector3d p(p_c.x, p_c.y, p_c.z); 1590 | // p = p_imu->Lid_rot_to_IMU * p + p_imu->Lid_offset_to_IMU; 1591 | p = state.rot_end * p + state.pos_end; 1592 | pcl::PointXYZI pi; 1593 | pi.x = p(0); 1594 | pi.y = p(1); 1595 | pi.z = p(2); 1596 | pi.intensity = p_c.intensity; 1597 | trans_cloud->points.push_back(pi); 1598 | } 1599 | } 1600 | 1601 | void build_single_residual(const pointWithCov& pv, 1602 | const OctoTree* current_octo, 1603 | const int current_layer, 1604 | const int max_layer, 1605 | const double sigma_num, 1606 | bool& is_sucess, 1607 | double& prob, 1608 | ptpl& single_ptpl) { 1609 | double radius_k = 3; 1610 | Eigen::Vector3d p_w = pv.point_world; 1611 | if (current_octo->plane_ptr_->is_plane) { 1612 | Plane& plane = *current_octo->plane_ptr_; 1613 | Eigen::Vector3d p_world_to_center = p_w - plane.center; 1614 | double proj_x = p_world_to_center.dot(plane.x_normal); 1615 | double proj_y = p_world_to_center.dot(plane.y_normal); 1616 | float dis_to_plane = fabs(plane.normal(0) * p_w(0) + plane.normal(1) * p_w(1) + 1617 | plane.normal(2) * p_w(2) + plane.d); 1618 | float dis_to_center = (plane.center(0) - p_w(0)) * (plane.center(0) - p_w(0)) + 1619 | (plane.center(1) - p_w(1)) * (plane.center(1) - p_w(1)) + 1620 | (plane.center(2) - p_w(2)) * (plane.center(2) - p_w(2)); 1621 | float range_dis = sqrt(dis_to_center - dis_to_plane * dis_to_plane); 1622 | 1623 | if (range_dis <= radius_k * plane.radius) { 1624 | Eigen::Matrix J_nq; 1625 | J_nq.block<1, 3>(0, 0) = p_w - plane.center; 1626 | J_nq.block<1, 3>(0, 3) = -plane.normal; 1627 | double sigma_l = J_nq * plane.plane_cov * J_nq.transpose(); 1628 | sigma_l += plane.normal.transpose() * pv.cov * plane.normal; 1629 | if (dis_to_plane < sigma_num * sqrt(sigma_l)) { 1630 | is_sucess = true; 1631 | double this_prob = 1.0 / (sqrt(sigma_l)) * 1632 | exp(-0.5 * dis_to_plane * dis_to_plane / sigma_l); 1633 | if (this_prob > prob) { 1634 | prob = this_prob; 1635 | single_ptpl.point = pv.point; 1636 | single_ptpl.plane_cov = plane.plane_cov; 1637 | single_ptpl.normal = plane.normal; 1638 | single_ptpl.center = plane.center; 1639 | single_ptpl.d = plane.d; 1640 | single_ptpl.layer = current_layer; 1641 | } 1642 | return; 1643 | } else { 1644 | // is_sucess = false; 1645 | return; 1646 | } 1647 | } else { 1648 | // is_sucess = false; 1649 | return; 1650 | } 1651 | } else { 1652 | if (current_layer < max_layer) { 1653 | for (size_t leafnum = 0; leafnum < 8; leafnum++) { 1654 | if (current_octo->leaves_[leafnum] != nullptr) { 1655 | OctoTree* leaf_octo = current_octo->leaves_[leafnum]; 1656 | build_single_residual(pv, 1657 | leaf_octo, 1658 | current_layer + 1, 1659 | max_layer, 1660 | sigma_num, 1661 | is_sucess, 1662 | prob, 1663 | single_ptpl); 1664 | } 1665 | } 1666 | return; 1667 | } else { 1668 | // is_sucess = false; 1669 | return; 1670 | } 1671 | } 1672 | } 1673 | 1674 | void GetUpdatePlane(const OctoTree* current_octo, 1675 | const int pub_max_voxel_layer, 1676 | std::vector& plane_list) { 1677 | if (current_octo->layer_ > pub_max_voxel_layer) { 1678 | return; 1679 | } 1680 | if (current_octo->plane_ptr_->is_update) { 1681 | plane_list.push_back(*current_octo->plane_ptr_); 1682 | } 1683 | if (current_octo->layer_ < current_octo->max_layer_) { 1684 | if (!current_octo->plane_ptr_->is_plane) { 1685 | for (size_t i = 0; i < 8; i++) { 1686 | if (current_octo->leaves_[i] != nullptr) { 1687 | GetUpdatePlane(current_octo->leaves_[i], pub_max_voxel_layer, plane_list); 1688 | } 1689 | } 1690 | } 1691 | } 1692 | return; 1693 | } 1694 | 1695 | // void BuildResidualListTBB(const unordered_map 1696 | // &voxel_map, 1697 | // const double voxel_size, const double sigma_num, 1698 | // const int max_layer, 1699 | // const std::vector &pv_list, 1700 | // std::vector &ptpl_list, 1701 | // std::vector &non_match) { 1702 | // std::mutex mylock; 1703 | // ptpl_list.clear(); 1704 | // std::vector all_ptpl_list(pv_list.size()); 1705 | // std::vector useful_ptpl(pv_list.size()); 1706 | // std::vector index(pv_list.size()); 1707 | // for (size_t i = 0; i < index.size(); ++i) { 1708 | // index[i] = i; 1709 | // useful_ptpl[i] = false; 1710 | // } 1711 | // std::for_each( 1712 | // std::execution::par_unseq, index.begin(), index.end(), 1713 | // [&](const size_t &i) { 1714 | // pointWithCov pv = pv_list[i]; 1715 | // float loc_xyz[3]; 1716 | // for (int j = 0; j < 3; j++) { 1717 | // loc_xyz[j] = pv.point_world[j] / voxel_size; 1718 | // if (loc_xyz[j] < 0) { 1719 | // loc_xyz[j] -= 1.0; 1720 | // } 1721 | // } 1722 | // VOXEL_LOC position((int64_t)loc_xyz[0], (int64_t)loc_xyz[1], 1723 | // (int64_t)loc_xyz[2]); 1724 | // auto iter = voxel_map.find(position); 1725 | // if (iter != voxel_map.end()) { 1726 | // OctoTree *current_octo = iter->second; 1727 | // ptpl single_ptpl; 1728 | // bool is_sucess = false; 1729 | // double prob = 0; 1730 | // build_single_residual(pv, current_octo, 0, max_layer, sigma_num, 1731 | // is_sucess, prob, single_ptpl); 1732 | // if (!is_sucess) { 1733 | // VOXEL_LOC near_position = position; 1734 | // if (loc_xyz[0] > (current_octo->voxel_center_[0] + 1735 | // current_octo->quater_length_)) { 1736 | // near_position.x = near_position.x + 1; 1737 | // } else if (loc_xyz[0] < (current_octo->voxel_center_[0] - 1738 | // current_octo->quater_length_)) { 1739 | // near_position.x = near_position.x - 1; 1740 | // } 1741 | // if (loc_xyz[1] > (current_octo->voxel_center_[1] + 1742 | // current_octo->quater_length_)) { 1743 | // near_position.y = near_position.y + 1; 1744 | // } else if (loc_xyz[1] < (current_octo->voxel_center_[1] - 1745 | // current_octo->quater_length_)) { 1746 | // near_position.y = near_position.y - 1; 1747 | // } 1748 | // if (loc_xyz[2] > (current_octo->voxel_center_[2] + 1749 | // current_octo->quater_length_)) { 1750 | // near_position.z = near_position.z + 1; 1751 | // } else if (loc_xyz[2] < (current_octo->voxel_center_[2] - 1752 | // current_octo->quater_length_)) { 1753 | // near_position.z = near_position.z - 1; 1754 | // } 1755 | // auto iter_near = voxel_map.find(near_position); 1756 | // if (iter_near != voxel_map.end()) { 1757 | // build_single_residual(pv, iter_near->second, 0, max_layer, 1758 | // sigma_num, is_sucess, prob, single_ptpl); 1759 | // } 1760 | // } 1761 | // if (is_sucess) { 1762 | 1763 | // mylock.lock(); 1764 | // useful_ptpl[i] = true; 1765 | // all_ptpl_list[i] = single_ptpl; 1766 | // mylock.unlock(); 1767 | // } else { 1768 | // mylock.lock(); 1769 | // useful_ptpl[i] = false; 1770 | // mylock.unlock(); 1771 | // } 1772 | // } 1773 | // }); 1774 | // for (size_t i = 0; i < useful_ptpl.size(); i++) { 1775 | // if (useful_ptpl[i]) { 1776 | // ptpl_list.push_back(all_ptpl_list[i]); 1777 | // } 1778 | // } 1779 | // } 1780 | 1781 | void BuildResidualListOMP(const unordered_map& voxel_map, 1782 | const double voxel_size, 1783 | const double sigma_num, 1784 | const int max_layer, 1785 | const std::vector& pv_list, 1786 | std::vector& ptpl_list, 1787 | std::vector& non_match) { 1788 | std::mutex mylock; 1789 | ptpl_list.clear(); 1790 | std::vector all_ptpl_list(pv_list.size()); 1791 | std::vector useful_ptpl(pv_list.size()); 1792 | std::vector index(pv_list.size()); 1793 | for (size_t i = 0; i < index.size(); ++i) { 1794 | index[i] = i; 1795 | useful_ptpl[i] = false; 1796 | } 1797 | #ifdef MP_EN 1798 | omp_set_num_threads(MP_PROC_NUM); 1799 | #pragma omp parallel for 1800 | #endif 1801 | for (int i = 0; i < index.size(); i++) { 1802 | pointWithCov pv = pv_list[i]; 1803 | float loc_xyz[3]; 1804 | for (int j = 0; j < 3; j++) { 1805 | loc_xyz[j] = pv.point_world[j] / voxel_size; 1806 | if (loc_xyz[j] < 0) { 1807 | loc_xyz[j] -= 1.0; 1808 | } 1809 | } 1810 | VOXEL_LOC position((int64_t) loc_xyz[0], (int64_t) loc_xyz[1], (int64_t) loc_xyz[2]); 1811 | auto iter = voxel_map.find(position); 1812 | if (iter != voxel_map.end()) { 1813 | OctoTree* current_octo = iter->second; 1814 | ptpl single_ptpl; 1815 | bool is_sucess = false; 1816 | double prob = 0; 1817 | build_single_residual(pv, 1818 | current_octo, 1819 | 0, 1820 | max_layer, 1821 | sigma_num, 1822 | is_sucess, 1823 | prob, 1824 | single_ptpl); 1825 | if (!is_sucess) { 1826 | VOXEL_LOC near_position = position; 1827 | if (loc_xyz[0] > (current_octo->voxel_center_[0] + current_octo->quater_length_)) { 1828 | near_position.x = near_position.x + 1; 1829 | } else if (loc_xyz[0] < (current_octo->voxel_center_[0] - current_octo->quater_length_)) { 1830 | near_position.x = near_position.x - 1; 1831 | } 1832 | if (loc_xyz[1] > (current_octo->voxel_center_[1] + current_octo->quater_length_)) { 1833 | near_position.y = near_position.y + 1; 1834 | } else if (loc_xyz[1] < (current_octo->voxel_center_[1] - current_octo->quater_length_)) { 1835 | near_position.y = near_position.y - 1; 1836 | } 1837 | if (loc_xyz[2] > (current_octo->voxel_center_[2] + current_octo->quater_length_)) { 1838 | near_position.z = near_position.z + 1; 1839 | } else if (loc_xyz[2] < (current_octo->voxel_center_[2] - current_octo->quater_length_)) { 1840 | near_position.z = near_position.z - 1; 1841 | } 1842 | auto iter_near = voxel_map.find(near_position); 1843 | if (iter_near != voxel_map.end()) { 1844 | build_single_residual(pv, 1845 | iter_near->second, 1846 | 0, 1847 | max_layer, 1848 | sigma_num, 1849 | is_sucess, 1850 | prob, 1851 | single_ptpl); 1852 | } 1853 | } 1854 | if (is_sucess) { 1855 | mylock.lock(); 1856 | useful_ptpl[i] = true; 1857 | all_ptpl_list[i] = single_ptpl; 1858 | mylock.unlock(); 1859 | } else { 1860 | mylock.lock(); 1861 | useful_ptpl[i] = false; 1862 | mylock.unlock(); 1863 | } 1864 | } 1865 | } 1866 | for (size_t i = 0; i < useful_ptpl.size(); i++) { 1867 | if (useful_ptpl[i]) { 1868 | ptpl_list.push_back(all_ptpl_list[i]); 1869 | } 1870 | } 1871 | } 1872 | 1873 | void BuildResidualListNormal(const unordered_map& voxel_map, 1874 | const double voxel_size, 1875 | const double sigma_num, 1876 | const int max_layer, 1877 | const std::vector& pv_list, 1878 | std::vector& ptpl_list, 1879 | std::vector& non_match) { 1880 | ptpl_list.clear(); 1881 | std::vector index(pv_list.size()); 1882 | for (size_t i = 0; i < pv_list.size(); ++i) { 1883 | pointWithCov pv = pv_list[i]; 1884 | float loc_xyz[3]; 1885 | for (int j = 0; j < 3; j++) { 1886 | loc_xyz[j] = pv.point_world[j] / voxel_size; 1887 | if (loc_xyz[j] < 0) { 1888 | loc_xyz[j] -= 1.0; 1889 | } 1890 | } 1891 | VOXEL_LOC position((int64_t) loc_xyz[0], (int64_t) loc_xyz[1], (int64_t) loc_xyz[2]); 1892 | auto iter = voxel_map.find(position); 1893 | if (iter != voxel_map.end()) { 1894 | OctoTree* current_octo = iter->second; 1895 | ptpl single_ptpl; 1896 | bool is_sucess = false; 1897 | double prob = 0; 1898 | build_single_residual(pv, 1899 | current_octo, 1900 | 0, 1901 | max_layer, 1902 | sigma_num, 1903 | is_sucess, 1904 | prob, 1905 | single_ptpl); 1906 | 1907 | if (!is_sucess) { 1908 | VOXEL_LOC near_position = position; 1909 | if (loc_xyz[0] > (current_octo->voxel_center_[0] + current_octo->quater_length_)) { 1910 | near_position.x = near_position.x + 1; 1911 | } else if (loc_xyz[0] < (current_octo->voxel_center_[0] - current_octo->quater_length_)) { 1912 | near_position.x = near_position.x - 1; 1913 | } 1914 | if (loc_xyz[1] > (current_octo->voxel_center_[1] + current_octo->quater_length_)) { 1915 | near_position.y = near_position.y + 1; 1916 | } else if (loc_xyz[1] < (current_octo->voxel_center_[1] - current_octo->quater_length_)) { 1917 | near_position.y = near_position.y - 1; 1918 | } 1919 | if (loc_xyz[2] > (current_octo->voxel_center_[2] + current_octo->quater_length_)) { 1920 | near_position.z = near_position.z + 1; 1921 | } else if (loc_xyz[2] < (current_octo->voxel_center_[2] - current_octo->quater_length_)) { 1922 | near_position.z = near_position.z - 1; 1923 | } 1924 | auto iter_near = voxel_map.find(near_position); 1925 | if (iter_near != voxel_map.end()) { 1926 | build_single_residual(pv, 1927 | iter_near->second, 1928 | 0, 1929 | max_layer, 1930 | sigma_num, 1931 | is_sucess, 1932 | prob, 1933 | single_ptpl); 1934 | } 1935 | } 1936 | if (is_sucess) { 1937 | ptpl_list.push_back(single_ptpl); 1938 | } else { 1939 | non_match.push_back(pv.point_world); 1940 | } 1941 | } 1942 | } 1943 | } 1944 | 1945 | void CalcVectQuation(const Eigen::Vector3d& x_vec, 1946 | const Eigen::Vector3d& y_vec, 1947 | const Eigen::Vector3d& z_vec, 1948 | geometry_msgs::Quaternion& q) { 1949 | Eigen::Matrix3d rot; 1950 | 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); 1951 | Eigen::Matrix3d rotation = rot.transpose(); 1952 | Eigen::Quaterniond eq(rotation); 1953 | q.w = eq.w(); 1954 | q.x = eq.x(); 1955 | q.y = eq.y(); 1956 | q.z = eq.z(); 1957 | } 1958 | 1959 | void CalcQuation(const Eigen::Vector3d& vec, const int axis, geometry_msgs::Quaternion& q) { 1960 | Eigen::Vector3d x_body = vec; 1961 | Eigen::Vector3d y_body(1, 1, 0); 1962 | if (x_body(2) != 0) { 1963 | y_body(2) = -(y_body(0) * x_body(0) + y_body(1) * x_body(1)) / x_body(2); 1964 | } else { 1965 | if (x_body(1) != 0) { 1966 | y_body(1) = -(y_body(0) * x_body(0)) / x_body(1); 1967 | } else { 1968 | y_body(0) = 0; 1969 | } 1970 | } 1971 | y_body.normalize(); 1972 | Eigen::Vector3d z_body = x_body.cross(y_body); 1973 | Eigen::Matrix3d rot; 1974 | 1975 | rot << x_body(0), x_body(1), x_body(2), y_body(0), y_body(1), y_body(2), z_body(0), z_body(1), 1976 | z_body(2); 1977 | Eigen::Matrix3d rotation = rot.transpose(); 1978 | if (axis == 2) { 1979 | Eigen::Matrix3d rot_inc; 1980 | rot_inc << 0, 0, 1, 0, 1, 0, -1, 0, 0; 1981 | rotation = rotation * rot_inc; 1982 | } 1983 | Eigen::Quaterniond eq(rotation); 1984 | q.w = eq.w(); 1985 | q.x = eq.x(); 1986 | q.y = eq.y(); 1987 | q.z = eq.z(); 1988 | } 1989 | 1990 | void pubSinglePlane(visualization_msgs::MarkerArray& plane_pub, 1991 | const std::string plane_ns, 1992 | const Plane& single_plane, 1993 | const float alpha, 1994 | const Eigen::Vector3d rgb) { 1995 | visualization_msgs::Marker plane; 1996 | plane.header.frame_id = "camera_init"; 1997 | plane.header.stamp = ros::Time(); 1998 | plane.ns = plane_ns; 1999 | plane.id = single_plane.id; 2000 | plane.type = visualization_msgs::Marker::CYLINDER; 2001 | plane.action = visualization_msgs::Marker::ADD; 2002 | plane.pose.position.x = single_plane.center[0]; 2003 | plane.pose.position.y = single_plane.center[1]; 2004 | plane.pose.position.z = single_plane.center[2]; 2005 | geometry_msgs::Quaternion q; 2006 | CalcVectQuation(single_plane.x_normal, single_plane.y_normal, single_plane.normal, q); 2007 | plane.pose.orientation = q; 2008 | plane.scale.x = 3 * sqrt(single_plane.max_eigen_value); 2009 | plane.scale.y = 3 * sqrt(single_plane.mid_eigen_value); 2010 | plane.scale.z = 2 * sqrt(single_plane.min_eigen_value); 2011 | plane.color.a = alpha; 2012 | plane.color.r = rgb(0); 2013 | plane.color.g = rgb(1); 2014 | plane.color.b = rgb(2); 2015 | plane.lifetime = ros::Duration(); 2016 | plane_pub.markers.push_back(plane); 2017 | } 2018 | 2019 | void pubNoPlaneMap(const std::unordered_map& feat_map, 2020 | const ros::Publisher& plane_map_pub) { 2021 | int id = 0; 2022 | ros::Rate loop(500); 2023 | float use_alpha = 0.8; 2024 | visualization_msgs::MarkerArray voxel_plane; 2025 | voxel_plane.markers.reserve(1000000); 2026 | for (auto iter = feat_map.begin(); iter != feat_map.end(); iter++) { 2027 | if (!iter->second->plane_ptr_->is_plane) { 2028 | for (uint i = 0; i < 8; i++) { 2029 | if (iter->second->leaves_[i] != nullptr) { 2030 | OctoTree* temp_octo_tree = iter->second->leaves_[i]; 2031 | if (!temp_octo_tree->plane_ptr_->is_plane) { 2032 | for (uint j = 0; j < 8; j++) { 2033 | if (temp_octo_tree->leaves_[j] != nullptr) { 2034 | if (!temp_octo_tree->leaves_[j]->plane_ptr_->is_plane) { 2035 | Eigen::Vector3d plane_rgb(1, 1, 1); 2036 | pubSinglePlane(voxel_plane, 2037 | "no_plane", 2038 | *(temp_octo_tree->leaves_[j]->plane_ptr_), 2039 | use_alpha, 2040 | plane_rgb); 2041 | } 2042 | } 2043 | } 2044 | } 2045 | } 2046 | } 2047 | } 2048 | } 2049 | plane_map_pub.publish(voxel_plane); 2050 | loop.sleep(); 2051 | } 2052 | 2053 | void pubVoxelMap(const std::unordered_map& voxel_map, 2054 | const int pub_max_voxel_layer, 2055 | const ros::Publisher& plane_map_pub) { 2056 | double max_trace = 0.25; 2057 | double pow_num = 0.2; 2058 | ros::Rate loop(500); 2059 | float use_alpha = 0.8; 2060 | visualization_msgs::MarkerArray voxel_plane; 2061 | voxel_plane.markers.reserve(1000000); 2062 | std::vector pub_plane_list; 2063 | for (auto iter = voxel_map.begin(); iter != voxel_map.end(); iter++) { 2064 | GetUpdatePlane(iter->second, pub_max_voxel_layer, pub_plane_list); 2065 | } 2066 | for (size_t i = 0; i < pub_plane_list.size(); i++) { 2067 | V3D plane_cov = pub_plane_list[i].plane_cov.block<3, 3>(0, 0).diagonal(); 2068 | double trace = plane_cov.sum(); 2069 | if (trace >= max_trace) { 2070 | trace = max_trace; 2071 | } 2072 | trace = trace * (1.0 / max_trace); 2073 | trace = pow(trace, pow_num); 2074 | uint8_t r, g, b; 2075 | mapJet(trace, 0, 1, r, g, b); 2076 | Eigen::Vector3d plane_rgb(r / 256.0, g / 256.0, b / 256.0); 2077 | double alpha; 2078 | if (pub_plane_list[i].is_plane) { 2079 | alpha = use_alpha; 2080 | } else { 2081 | alpha = 0; 2082 | } 2083 | pubSinglePlane(voxel_plane, "plane", pub_plane_list[i], alpha, plane_rgb); 2084 | } 2085 | plane_map_pub.publish(voxel_plane); 2086 | loop.sleep(); 2087 | } 2088 | 2089 | void pubPlaneMap(const std::unordered_map& feat_map, 2090 | const ros::Publisher& plane_map_pub) { 2091 | OctoTree* current_octo = nullptr; 2092 | 2093 | double max_trace = 0.25; 2094 | double pow_num = 0.2; 2095 | ros::Rate loop(500); 2096 | float use_alpha = 1.0; 2097 | visualization_msgs::MarkerArray voxel_plane; 2098 | voxel_plane.markers.reserve(1000000); 2099 | 2100 | for (auto iter = feat_map.begin(); iter != feat_map.end(); iter++) { 2101 | if (iter->second->plane_ptr_->is_update) { 2102 | Eigen::Vector3d normal_rgb(0.0, 1.0, 0.0); 2103 | 2104 | V3D plane_cov = iter->second->plane_ptr_->plane_cov.block<3, 3>(0, 0).diagonal(); 2105 | double trace = plane_cov.sum(); 2106 | if (trace >= max_trace) { 2107 | trace = max_trace; 2108 | } 2109 | trace = trace * (1.0 / max_trace); 2110 | trace = pow(trace, pow_num); 2111 | uint8_t r, g, b; 2112 | mapJet(trace, 0, 1, r, g, b); 2113 | Eigen::Vector3d plane_rgb(r / 256.0, g / 256.0, b / 256.0); 2114 | // Eigen::Vector3d plane_rgb(1, 0, 0); 2115 | float alpha = 0.0; 2116 | if (iter->second->plane_ptr_->is_plane) { 2117 | alpha = use_alpha; 2118 | } else { 2119 | // std::cout << "delete plane" << std::endl; 2120 | } 2121 | pubSinglePlane(voxel_plane, "plane", *(iter->second->plane_ptr_), alpha, plane_rgb); 2122 | 2123 | iter->second->plane_ptr_->is_update = false; 2124 | } else { 2125 | for (uint i = 0; i < 8; i++) { 2126 | if (iter->second->leaves_[i] != nullptr) { 2127 | if (iter->second->leaves_[i]->plane_ptr_->is_update) { 2128 | Eigen::Vector3d normal_rgb(0.0, 1.0, 0.0); 2129 | 2130 | V3D plane_cov = iter->second->leaves_[i] 2131 | ->plane_ptr_->plane_cov.block<3, 3>(0, 0) 2132 | .diagonal(); 2133 | double trace = plane_cov.sum(); 2134 | if (trace >= max_trace) { 2135 | trace = max_trace; 2136 | } 2137 | trace = trace * (1.0 / max_trace); 2138 | // trace = (max_trace - trace) / max_trace; 2139 | trace = pow(trace, pow_num); 2140 | uint8_t r, g, b; 2141 | mapJet(trace, 0, 1, r, g, b); 2142 | Eigen::Vector3d plane_rgb(r / 256.0, g / 256.0, b / 256.0); 2143 | plane_rgb << 0, 1, 0; 2144 | // fabs(iter->second->leaves_[i]->plane_ptr_->normal[0]), 2145 | // fabs(iter->second->leaves_[i]->plane_ptr_->normal[1]), 2146 | // fabs(iter->second->leaves_[i]->plane_ptr_->normal[2]); 2147 | float alpha = 0.0; 2148 | if (iter->second->leaves_[i]->plane_ptr_->is_plane) { 2149 | alpha = use_alpha; 2150 | } else { 2151 | // std::cout << "delete plane" << std::endl; 2152 | } 2153 | pubSinglePlane(voxel_plane, 2154 | "plane", 2155 | *(iter->second->leaves_[i]->plane_ptr_), 2156 | alpha, 2157 | plane_rgb); 2158 | // loop.sleep(); 2159 | iter->second->leaves_[i]->plane_ptr_->is_update = false; 2160 | // loop.sleep(); 2161 | } else { 2162 | OctoTree* temp_octo_tree = iter->second->leaves_[i]; 2163 | for (uint j = 0; j < 8; j++) { 2164 | if (temp_octo_tree->leaves_[j] != nullptr) { 2165 | if (temp_octo_tree->leaves_[j]->octo_state_ == 0 && 2166 | temp_octo_tree->leaves_[j]->plane_ptr_->is_update) { 2167 | if (temp_octo_tree->leaves_[j]->plane_ptr_->is_plane) { 2168 | // std::cout << "subsubplane" << std::endl; 2169 | Eigen::Vector3d normal_rgb(0.0, 1.0, 0.0); 2170 | V3D plane_cov = temp_octo_tree->leaves_[j] 2171 | ->plane_ptr_->plane_cov.block<3, 3>(0, 0) 2172 | .diagonal(); 2173 | double trace = plane_cov.sum(); 2174 | if (trace >= max_trace) { 2175 | trace = max_trace; 2176 | } 2177 | trace = trace * (1.0 / max_trace); 2178 | // trace = (max_trace - trace) / max_trace; 2179 | trace = pow(trace, pow_num); 2180 | uint8_t r, g, b; 2181 | mapJet(trace, 0, 1, r, g, b); 2182 | Eigen::Vector3d plane_rgb(r / 256.0, g / 256.0, b / 256.0); 2183 | plane_rgb << 0, 0, 1; 2184 | float alpha = 0.0; 2185 | if (temp_octo_tree->leaves_[j]->plane_ptr_->is_plane) { 2186 | alpha = use_alpha; 2187 | } 2188 | 2189 | pubSinglePlane(voxel_plane, 2190 | "plane", 2191 | *(temp_octo_tree->leaves_[j]->plane_ptr_), 2192 | alpha, 2193 | plane_rgb); 2194 | // loop.sleep(); 2195 | temp_octo_tree->leaves_[j]->plane_ptr_->is_update = false; 2196 | } 2197 | } 2198 | } 2199 | } 2200 | } 2201 | } 2202 | } 2203 | } 2204 | } 2205 | 2206 | plane_map_pub.publish(voxel_plane); 2207 | // plane_map_pub.publish(voxel_norm); 2208 | loop.sleep(); 2209 | // cout << "[Map Info] Plane counts:" << plane_count 2210 | // << " Sub Plane counts:" << sub_plane_count 2211 | // << " Sub Sub Plane counts:" << sub_sub_plane_count << endl; 2212 | // cout << "[Map Info] Update plane counts:" << update_count 2213 | // << "total size: " << feat_map.size() << endl; 2214 | } 2215 | 2216 | std::tuple GenerateBoundaryRGB(size_t idx) { 2217 | std::hash hasher; 2218 | std::mt19937 rng(hasher(idx)); 2219 | std::uniform_int_distribution dist(0, 255); 2220 | 2221 | int r = dist(rng); 2222 | int g = dist(rng); 2223 | int b = dist(rng); 2224 | 2225 | return std::make_tuple(r, g, b); 2226 | } 2227 | 2228 | void SetVoxelPlaneVisualizationInfo(visualization_msgs::MarkerArray& plane_pub, 2229 | const Plane* const single_plane, 2230 | const Eigen::Vector3d& plane_position, 2231 | double plane_scale, 2232 | int viz_plane_id) { 2233 | visualization_msgs::Marker plane; 2234 | plane.header.frame_id = "camera_init"; 2235 | plane.header.stamp = ros::Time(); 2236 | plane.ns = "merged voxel"; 2237 | plane.id = viz_plane_id; 2238 | plane.type = visualization_msgs::Marker::CYLINDER; 2239 | plane.action = visualization_msgs::Marker::ADD; 2240 | 2241 | auto color = GenerateBoundaryRGB(single_plane->merge_plane_id); 2242 | int r = std::get<0>(color); 2243 | int g = std::get<1>(color); 2244 | int b = std::get<2>(color); 2245 | 2246 | Eigen::Vector3f plane_rgb(r / 256.f, g / 256.f, b / 256.f); 2247 | 2248 | plane.color.r = plane_rgb(0); 2249 | plane.color.g = plane_rgb(1); 2250 | plane.color.b = plane_rgb(2); 2251 | 2252 | plane.pose.position.x = plane_position[0]; 2253 | plane.pose.position.y = plane_position[1]; 2254 | plane.pose.position.z = plane_position[2]; 2255 | 2256 | const auto& plane_normal = single_plane->normal.normalized(); 2257 | Eigen::Vector3d base(0, 0, 1); 2258 | size_t max_index = 0; 2259 | plane_normal.cwiseAbs().maxCoeff(&max_index); 2260 | if (max_index == 2) { 2261 | base << 1.f, 0.f, 0.f; 2262 | } 2263 | Eigen::Vector3d x_normal = (plane_normal.cross(base)).normalized(); 2264 | Eigen::Vector3d y_normal = plane_normal.cross(x_normal); 2265 | 2266 | geometry_msgs::Quaternion q; 2267 | CalcVectQuation(x_normal, y_normal, plane_normal, q); 2268 | 2269 | plane.pose.orientation = q; 2270 | plane.scale.x = plane_scale; 2271 | plane.scale.y = plane_scale; 2272 | plane.scale.z = 0.01; 2273 | plane.color.a = 0.8; 2274 | 2275 | plane_pub.markers.push_back(plane); 2276 | } 2277 | 2278 | void PublishMergedVoxelPlane( 2279 | std::unordered_map& feat_map, 2280 | const std::unordered_map>& merged_voxels, 2281 | const ros::Publisher& plane_map_pub, 2282 | double voxel_size) { 2283 | if (merged_voxels.empty()) { 2284 | return; 2285 | } 2286 | 2287 | visualization_msgs::MarkerArray voxel_plane; 2288 | voxel_plane.markers.reserve(1000000); 2289 | 2290 | int viz_plane_id = 0; 2291 | 2292 | for (const auto& plane_i : merged_voxels) { 2293 | if (plane_i.second.empty()) { 2294 | continue; 2295 | } 2296 | 2297 | auto node_loc_it = *(plane_i.second.begin()); 2298 | 2299 | const OctoTree* const current_node = GetVoxelPlaneNode(feat_map, node_loc_it); 2300 | 2301 | auto current_plane = current_node->plane_ptr_; 2302 | 2303 | for (const auto& content_loc : plane_i.second) { 2304 | VOXEL_LOC v(content_loc.x, content_loc.y, content_loc.z); 2305 | 2306 | double plane_scale = voxel_size; 2307 | 2308 | Eigen::Vector3d viz_center; 2309 | viz_center << (0.5f + v.x) * plane_scale, (0.5f + v.y) * plane_scale, 2310 | (0.5f + v.z) * plane_scale; 2311 | 2312 | Eigen::Vector3d n = current_plane->normal; 2313 | 2314 | double t = n.dot(current_plane->center) - n.dot(viz_center); 2315 | 2316 | Eigen::Vector3d plane_pose = viz_center + t * n; 2317 | 2318 | if (content_loc.leaf_num > 0) { 2319 | plane_scale *= 0.5f; 2320 | } 2321 | 2322 | SetVoxelPlaneVisualizationInfo(voxel_plane, 2323 | current_plane, 2324 | plane_pose, 2325 | plane_scale, 2326 | viz_plane_id); 2327 | 2328 | viz_plane_id++; 2329 | } 2330 | } 2331 | plane_map_pub.publish(voxel_plane); 2332 | } 2333 | 2334 | void calcBodyCov(Eigen::Vector3d& pb, 2335 | const float range_inc, 2336 | const float degree_inc, 2337 | Eigen::Matrix3d& cov) { 2338 | float range = sqrt(pb[0] * pb[0] + pb[1] * pb[1] + pb[2] * pb[2]); 2339 | float range_var = range_inc * range_inc; 2340 | Eigen::Matrix2d direction_var; 2341 | direction_var << pow(sin(DEG2RAD(degree_inc)), 2), 0, 0, pow(sin(DEG2RAD(degree_inc)), 2); 2342 | Eigen::Vector3d direction(pb); 2343 | direction.normalize(); 2344 | Eigen::Matrix3d direction_hat; 2345 | direction_hat << 0, -direction(2), direction(1), direction(2), 0, -direction(0), -direction(1), 2346 | direction(0), 0; 2347 | Eigen::Vector3d base_vector1(1, 1, -(direction(0) + direction(1)) / direction(2)); 2348 | base_vector1.normalize(); 2349 | Eigen::Vector3d base_vector2 = base_vector1.cross(direction); 2350 | base_vector2.normalize(); 2351 | Eigen::Matrix N; 2352 | N << base_vector1(0), base_vector2(0), base_vector1(1), base_vector2(1), base_vector1(2), 2353 | base_vector2(2); 2354 | Eigen::Matrix A = range * direction_hat * N; 2355 | cov = direction * range_var * direction.transpose() + A * direction_var * A.transpose(); 2356 | }; 2357 | 2358 | #endif -------------------------------------------------------------------------------- /launch/mapping_l515.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /launch/mapping_velodyne.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /msg/Pose6D.msg: -------------------------------------------------------------------------------- 1 | # the preintegrated Lidar states at the time of IMU measurements in a frame 2 | float64 offset_time # the offset time of IMU measurement w.r.t the first lidar point 3 | float64[3] acc # the preintegrated total acceleration (global frame) at the Lidar origin 4 | float64[3] gyr # the unbiased angular velocity (body frame) at the Lidar origin 5 | float64[3] vel # the preintegrated velocity (global frame) at the Lidar origin 6 | float64[3] pos # the preintegrated position (global frame) at the Lidar origin 7 | float64[9] rot # the preintegrated rotation (global frame) at the Lidar origin -------------------------------------------------------------------------------- /msg/States.msg: -------------------------------------------------------------------------------- 1 | Header header # timestamp of the first lidar in a frame 2 | float64[] rot_end # the estimated attitude (rotation matrix) at the end lidar point 3 | float64[] pos_end # the estimated position at the end lidar point (world frame) 4 | float64[] vel_end # the estimated velocity at the end lidar point (world frame) 5 | float64[] bias_gyr # gyroscope bias 6 | float64[] bias_acc # accelerator bias 7 | float64[] gravity # the estimated gravity acceleration 8 | float64[] cov # states covariance 9 | # Pose6D[] IMUpose # 6D pose at each imu measurements -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | c3p_voxelmap 4 | 1.0.0 5 | 6 | 7 | C3P-VoxelMap is a probabilistic voxel mapping method based on VoxelMap, 8 | enhancing performance, accuracy, and memory efficiency in LiDAR odometry. 9 | 10 | 11 | Xu Yang 12 | 13 | BSD 14 | 15 | Xu Yang 16 | 17 | catkin 18 | geometry_msgs 19 | nav_msgs 20 | roscpp 21 | rospy 22 | std_msgs 23 | sensor_msgs 24 | tf 25 | pcl_ros 26 | livox_ros_driver 27 | message_generation 28 | 29 | geometry_msgs 30 | nav_msgs 31 | sensor_msgs 32 | roscpp 33 | rospy 34 | std_msgs 35 | tf 36 | pcl_ros 37 | livox_ros_driver 38 | message_runtime 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /rviz_cfg/c3p_voxelmap.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /mapping1/surround1 9 | - /mapping1/currPoints1 10 | - /mapping1/currPoints1/Autocompute Value Bounds1 11 | - /Odometry1/Odometry1 12 | - /Odometry1/Odometry1/Shape1 13 | - /Odometry1/Odometry2 14 | - /Odometry1/Odometry2/Shape1 15 | - /Odometry2/Shape1 16 | Splitter Ratio: 0.520588219165802 17 | Tree Height: 653 18 | - Class: rviz/Selection 19 | Name: Selection 20 | - Class: rviz/Tool Properties 21 | Expanded: 22 | - /2D Pose Estimate1 23 | - /2D Nav Goal1 24 | - /Publish Point1 25 | Name: Tool Properties 26 | Splitter Ratio: 0.5886790156364441 27 | - Class: rviz/Views 28 | Expanded: 29 | - /Current View1 30 | Name: Views 31 | Splitter Ratio: 0.5 32 | - Class: rviz/Time 33 | Name: Time 34 | SyncMode: 0 35 | SyncSource: currPoints 36 | Preferences: 37 | PromptSaveOnExit: true 38 | Toolbars: 39 | toolButtonStyle: 2 40 | Visualization Manager: 41 | Class: "" 42 | Displays: 43 | - Alpha: 1 44 | Cell Size: 3 45 | Class: rviz/Grid 46 | Color: 160; 160; 164 47 | Enabled: false 48 | Line Style: 49 | Line Width: 0.029999999329447746 50 | Value: Lines 51 | Name: Grid 52 | Normal Cell Count: 0 53 | Offset: 54 | X: 0 55 | Y: 0 56 | Z: 0 57 | Plane: XY 58 | Plane Cell Count: 1000 59 | Reference Frame: 60 | Value: false 61 | - Alpha: 1 62 | Class: rviz/Axes 63 | Enabled: true 64 | Length: 0.699999988079071 65 | Name: Axes 66 | Radius: 0.05999999865889549 67 | Reference Frame: 68 | Show Trail: false 69 | Value: true 70 | - Class: rviz/Group 71 | Displays: 72 | - Alpha: 1 73 | Autocompute Intensity Bounds: false 74 | Autocompute Value Bounds: 75 | Max Value: 10 76 | Min Value: -10 77 | Value: true 78 | Axis: Z 79 | Channel Name: intensity 80 | Class: rviz/PointCloud2 81 | Color: 255; 255; 255 82 | Color Transformer: FlatColor 83 | Decay Time: 0 84 | Enabled: false 85 | Invert Rainbow: false 86 | Max Color: 255; 255; 255 87 | Max Intensity: 1 88 | Min Color: 0; 0; 0 89 | Min Intensity: 0 90 | Name: surround 91 | Position Transformer: XYZ 92 | Queue Size: 1 93 | Selectable: false 94 | Size (Pixels): 5 95 | Size (m): 0.05000000074505806 96 | Style: Points 97 | Topic: /cloud_registered 98 | Unreliable: false 99 | Use Fixed Frame: true 100 | Use rainbow: true 101 | Value: false 102 | - Alpha: 0.5 103 | Autocompute Intensity Bounds: true 104 | Autocompute Value Bounds: 105 | Max Value: 6.024350166320801 106 | Min Value: -1.9103225469589233 107 | Value: true 108 | Axis: Z 109 | Channel Name: intensity 110 | Class: rviz/PointCloud2 111 | Color: 255; 255; 255 112 | Color Transformer: AxisColor 113 | Decay Time: 99999 114 | Enabled: true 115 | Invert Rainbow: true 116 | Max Color: 255; 255; 255 117 | Min Color: 0; 0; 0 118 | Name: currPoints 119 | Position Transformer: XYZ 120 | Queue Size: 100000 121 | Selectable: true 122 | Size (Pixels): 1 123 | Size (m): 0.009999999776482582 124 | Style: Points 125 | Topic: /cloud_registered 126 | Unreliable: false 127 | Use Fixed Frame: true 128 | Use rainbow: true 129 | Value: true 130 | Enabled: true 131 | Name: mapping 132 | - Class: rviz/Group 133 | Displays: 134 | - Angle Tolerance: 0.009999999776482582 135 | Class: rviz/Odometry 136 | Covariance: 137 | Orientation: 138 | Alpha: 0.5 139 | Color: 255; 255; 127 140 | Color Style: Unique 141 | Frame: Local 142 | Offset: 1 143 | Scale: 1 144 | Value: true 145 | Position: 146 | Alpha: 0.30000001192092896 147 | Color: 204; 51; 204 148 | Scale: 1 149 | Value: true 150 | Value: true 151 | Enabled: false 152 | Keep: 1 153 | Name: Odometry 154 | Position Tolerance: 0.009999999776482582 155 | Queue Size: 10 156 | Shape: 157 | Alpha: 1 158 | Axes Length: 0.5 159 | Axes Radius: 0.009999999776482582 160 | Color: 0; 255; 0 161 | Head Length: 0 162 | Head Radius: 0 163 | Shaft Length: 0.05000000074505806 164 | Shaft Radius: 0.05000000074505806 165 | Value: Arrow 166 | Topic: /re_local_pose 167 | Unreliable: false 168 | Value: false 169 | - Angle Tolerance: 0.009999999776482582 170 | Class: rviz/Odometry 171 | Covariance: 172 | Orientation: 173 | Alpha: 0.5 174 | Color: 255; 255; 127 175 | Color Style: Unique 176 | Frame: Local 177 | Offset: 1 178 | Scale: 1 179 | Value: true 180 | Position: 181 | Alpha: 0.30000001192092896 182 | Color: 204; 51; 204 183 | Scale: 1 184 | Value: true 185 | Value: true 186 | Enabled: true 187 | Keep: 1 188 | Name: Odometry 189 | Position Tolerance: 0.0010000000474974513 190 | Queue Size: 10 191 | Shape: 192 | Alpha: 1 193 | Axes Length: 0.699999988079071 194 | Axes Radius: 0.10000000149011612 195 | Color: 255; 85; 0 196 | Head Length: 0 197 | Head Radius: 0 198 | Shaft Length: 0.05000000074505806 199 | Shaft Radius: 0.05000000074505806 200 | Value: Axes 201 | Topic: /aft_mapped_to_init 202 | Unreliable: false 203 | Value: true 204 | Enabled: true 205 | Name: Odometry 206 | - Alpha: 1 207 | Class: rviz/Axes 208 | Enabled: true 209 | Length: 0.699999988079071 210 | Name: Axes 211 | Radius: 0.10000000149011612 212 | Reference Frame: aft_mapped 213 | Show Trail: false 214 | Value: true 215 | - Alpha: 0 216 | Buffer Length: 2 217 | Class: rviz/Path 218 | Color: 25; 255; 255 219 | Enabled: true 220 | Head Diameter: 0 221 | Head Length: 0.10000000149011612 222 | Length: 0.5 223 | Line Style: Lines 224 | Line Width: 0.029999999329447746 225 | Name: Path 226 | Offset: 227 | X: 0 228 | Y: 0 229 | Z: 0 230 | Pose Color: 25; 255; 255 231 | Pose Style: None 232 | Queue Size: 10 233 | Radius: 0.20000000298023224 234 | Shaft Diameter: 0.10000000149011612 235 | Shaft Length: 0.5 236 | Topic: /path 237 | Unreliable: false 238 | Value: true 239 | - Class: rviz/Marker 240 | Enabled: true 241 | Marker Topic: /planner_normal 242 | Name: Marker 243 | Namespaces: 244 | {} 245 | Queue Size: 100 246 | Value: true 247 | - Alpha: 1 248 | Autocompute Intensity Bounds: true 249 | Autocompute Value Bounds: 250 | Max Value: 10 251 | Min Value: -10 252 | Value: true 253 | Axis: Z 254 | Channel Name: intensity 255 | Class: rviz/PointCloud2 256 | Color: 255; 0; 0 257 | Color Transformer: Intensity 258 | Decay Time: 0 259 | Enabled: true 260 | Invert Rainbow: false 261 | Max Color: 255; 255; 255 262 | Min Color: 0; 0; 0 263 | Name: PointCloud2 264 | Position Transformer: XYZ 265 | Queue Size: 10 266 | Selectable: true 267 | Size (Pixels): 3 268 | Size (m): 0.05000000074505806 269 | Style: Points 270 | Topic: /cloud_effected 271 | Unreliable: false 272 | Use Fixed Frame: true 273 | Use rainbow: true 274 | Value: true 275 | - Class: rviz/MarkerArray 276 | Enabled: true 277 | Marker Topic: /points_with_vars 278 | Name: MarkerArray 279 | Namespaces: 280 | {} 281 | Queue Size: 10000 282 | Value: true 283 | - Alpha: 1 284 | Autocompute Intensity Bounds: true 285 | Autocompute Value Bounds: 286 | Max Value: 10 287 | Min Value: -10 288 | Value: true 289 | Axis: Z 290 | Channel Name: intensity 291 | Class: rviz/PointCloud2 292 | Color: 170; 255; 0 293 | Color Transformer: FlatColor 294 | Decay Time: 0 295 | Enabled: true 296 | Invert Rainbow: false 297 | Max Color: 255; 255; 255 298 | Min Color: 0; 0; 0 299 | Name: PointCloud2 300 | Position Transformer: XYZ 301 | Queue Size: 10 302 | Selectable: true 303 | Size (Pixels): 3 304 | Size (m): 0.029999999329447746 305 | Style: Flat Squares 306 | Topic: /transform_cloud 307 | Unreliable: false 308 | Use Fixed Frame: true 309 | Use rainbow: true 310 | Value: true 311 | - Alpha: 1 312 | Autocompute Intensity Bounds: true 313 | Autocompute Value Bounds: 314 | Max Value: 10 315 | Min Value: -10 316 | Value: true 317 | Axis: Z 318 | Channel Name: intensity 319 | Class: rviz/PointCloud2 320 | Color: 255; 255; 255 321 | Color Transformer: FlatColor 322 | Decay Time: 0 323 | Enabled: true 324 | Invert Rainbow: false 325 | Max Color: 255; 255; 255 326 | Min Color: 0; 0; 0 327 | Name: PointCloud2 328 | Position Transformer: XYZ 329 | Queue Size: 10 330 | Selectable: true 331 | Size (Pixels): 3 332 | Size (m): 0.029999999329447746 333 | Style: Flat Squares 334 | Topic: /voxel_cloud 335 | Unreliable: false 336 | Use Fixed Frame: true 337 | Use rainbow: true 338 | Value: true 339 | - Alpha: 0.20000000298023224 340 | Autocompute Intensity Bounds: true 341 | Autocompute Value Bounds: 342 | Max Value: 10 343 | Min Value: -10 344 | Value: true 345 | Axis: Z 346 | Channel Name: intensity 347 | Class: rviz/PointCloud2 348 | Color: 255; 255; 255 349 | Color Transformer: FlatColor 350 | Decay Time: 0 351 | Enabled: false 352 | Invert Rainbow: false 353 | Max Color: 255; 255; 255 354 | Min Color: 0; 0; 0 355 | Name: PointCloud2 356 | Position Transformer: XYZ 357 | Queue Size: 10 358 | Selectable: true 359 | Size (Pixels): 3 360 | Size (m): 0.30000001192092896 361 | Style: Flat Squares 362 | Topic: /cloud_low_intens 363 | Unreliable: false 364 | Use Fixed Frame: true 365 | Use rainbow: true 366 | Value: false 367 | - Angle Tolerance: 0.10000000149011612 368 | Class: rviz/Odometry 369 | Covariance: 370 | Orientation: 371 | Alpha: 0.5 372 | Color: 255; 255; 127 373 | Color Style: Unique 374 | Frame: Local 375 | Offset: 1 376 | Scale: 1 377 | Value: true 378 | Position: 379 | Alpha: 0.30000001192092896 380 | Color: 204; 51; 204 381 | Scale: 1 382 | Value: true 383 | Value: true 384 | Enabled: false 385 | Keep: 10000 386 | Name: Odometry 387 | Position Tolerance: 0.10000000149011612 388 | Queue Size: 10 389 | Shape: 390 | Alpha: 1 391 | Axes Length: 10 392 | Axes Radius: 0.10000000149011612 393 | Color: 255; 25; 0 394 | Head Length: 0.30000001192092896 395 | Head Radius: 0.10000000149011612 396 | Shaft Length: 1 397 | Shaft Radius: 0.05000000074505806 398 | Value: Axes 399 | Topic: /odometry_gt 400 | Unreliable: false 401 | Value: false 402 | - Class: rviz/Image 403 | Enabled: true 404 | Image Topic: /camera/color/image_raw 405 | Max Value: 1 406 | Median window: 5 407 | Min Value: 0 408 | Name: Image 409 | Normalize Range: true 410 | Queue Size: 2 411 | Transport Hint: raw 412 | Unreliable: false 413 | Value: true 414 | - Class: rviz/MarkerArray 415 | Enabled: true 416 | Marker Topic: /planes 417 | Name: MarkerArray 418 | Namespaces: 419 | {} 420 | Queue Size: 100 421 | Value: true 422 | - Alpha: 1 423 | Autocompute Intensity Bounds: true 424 | Autocompute Value Bounds: 425 | Max Value: 10 426 | Min Value: -10 427 | Value: true 428 | Axis: Z 429 | Channel Name: intensity 430 | Class: rviz/PointCloud2 431 | Color: 255; 255; 255 432 | Color Transformer: FlatColor 433 | Decay Time: 0 434 | Enabled: true 435 | Invert Rainbow: false 436 | Max Color: 255; 255; 255 437 | Min Color: 0; 0; 0 438 | Name: PointCloud2 439 | Position Transformer: XYZ 440 | Queue Size: 10 441 | Selectable: true 442 | Size (Pixels): 5 443 | Size (m): 0.009999999776482582 444 | Style: Points 445 | Topic: /cloud_no_effected 446 | Unreliable: false 447 | Use Fixed Frame: true 448 | Use rainbow: true 449 | Value: true 450 | - Alpha: 1 451 | Autocompute Intensity Bounds: true 452 | Autocompute Value Bounds: 453 | Max Value: 10 454 | Min Value: -10 455 | Value: true 456 | Axis: Z 457 | Channel Name: intensity 458 | Class: rviz/PointCloud2 459 | Color: 239; 41; 41 460 | Color Transformer: FlatColor 461 | Decay Time: 0 462 | Enabled: false 463 | Invert Rainbow: false 464 | Max Color: 255; 255; 255 465 | Min Color: 0; 0; 0 466 | Name: PointCloud2 467 | Position Transformer: XYZ 468 | Queue Size: 10 469 | Selectable: true 470 | Size (Pixels): 3 471 | Size (m): 0.009999999776482582 472 | Style: Points 473 | Topic: /cloud_error 474 | Unreliable: false 475 | Use Fixed Frame: true 476 | Use rainbow: true 477 | Value: false 478 | - Class: rviz/Image 479 | Enabled: false 480 | Image Topic: /camera/color/image_raw 481 | Max Value: 1 482 | Median window: 5 483 | Min Value: 0 484 | Name: Image 485 | Normalize Range: true 486 | Queue Size: 2 487 | Transport Hint: raw 488 | Unreliable: false 489 | Value: false 490 | - Alpha: 1 491 | Autocompute Intensity Bounds: true 492 | Autocompute Value Bounds: 493 | Max Value: 10 494 | Min Value: -10 495 | Value: true 496 | Axis: Z 497 | Channel Name: intensity 498 | Class: rviz/PointCloud2 499 | Color: 255; 255; 255 500 | Color Transformer: Intensity 501 | Decay Time: 0 502 | Enabled: true 503 | Invert Rainbow: false 504 | Max Color: 255; 255; 255 505 | Min Color: 0; 0; 0 506 | Name: pgoMap 507 | Position Transformer: XYZ 508 | Queue Size: 10 509 | Selectable: true 510 | Size (Pixels): 3 511 | Size (m): 0.009999999776482582 512 | Style: Flat Squares 513 | Topic: /aft_pgo_map 514 | Unreliable: false 515 | Use Fixed Frame: true 516 | Use rainbow: true 517 | Value: true 518 | - Alpha: 1 519 | Buffer Length: 1 520 | Class: rviz/Path 521 | Color: 255; 0; 0 522 | Enabled: true 523 | Head Diameter: 0.30000001192092896 524 | Head Length: 0.20000000298023224 525 | Length: 0.30000001192092896 526 | Line Style: Lines 527 | Line Width: 0.029999999329447746 528 | Name: pgoPath 529 | Offset: 530 | X: 0 531 | Y: 0 532 | Z: 0 533 | Pose Color: 255; 85; 255 534 | Pose Style: None 535 | Queue Size: 10 536 | Radius: 0.029999999329447746 537 | Shaft Diameter: 0.10000000149011612 538 | Shaft Length: 0.10000000149011612 539 | Topic: /aft_pgo_path 540 | Unreliable: false 541 | Value: true 542 | - Class: rviz/MarkerArray 543 | Enabled: true 544 | Marker Topic: /plane_hash_map 545 | Name: PlaneHashMap 546 | Namespaces: 547 | {} 548 | Queue Size: 100 549 | Value: true 550 | - Class: rviz/MarkerArray 551 | Enabled: true 552 | Marker Topic: /merged_voxels 553 | Name: MarkerArray 554 | Namespaces: 555 | merged voxel: true 556 | Queue Size: 100 557 | Value: true 558 | Enabled: true 559 | Global Options: 560 | Background Color: 0; 0; 0 561 | Default Light: true 562 | Fixed Frame: camera_init 563 | Frame Rate: 30 564 | Name: root 565 | Tools: 566 | - Class: rviz/Interact 567 | Hide Inactive Objects: true 568 | - Class: rviz/MoveCamera 569 | - Class: rviz/Select 570 | - Class: rviz/FocusCamera 571 | - Class: rviz/Measure 572 | - Class: rviz/SetInitialPose 573 | Theta std deviation: 0.2617993950843811 574 | Topic: /initialpose 575 | X std deviation: 0.5 576 | Y std deviation: 0.5 577 | - Class: rviz/SetGoal 578 | Topic: /move_base_simple/goal 579 | - Class: rviz/PublishPoint 580 | Single click: true 581 | Topic: /clicked_point 582 | Value: true 583 | Views: 584 | Current: 585 | Class: rviz/Orbit 586 | Distance: 296.3080139160156 587 | Enable Stereo Rendering: 588 | Stereo Eye Separation: 0.05999999865889549 589 | Stereo Focal Distance: 1 590 | Swap Stereo Eyes: false 591 | Value: false 592 | Field of View: 0.7853981852531433 593 | Focal Point: 594 | X: 135.8769073486328 595 | Y: -10.643716812133789 596 | Z: 3.868349552154541 597 | Focal Shape Fixed Size: false 598 | Focal Shape Size: 0.05000000074505806 599 | Invert Z Axis: false 600 | Name: Current View 601 | Near Clip Distance: 0.009999999776482582 602 | Pitch: 1.5297963619232178 603 | Target Frame: camera_init 604 | Yaw: 1.458579182624817 605 | Saved: 606 | - Class: rviz/Orbit 607 | Distance: 310.7291564941406 608 | Enable Stereo Rendering: 609 | Stereo Eye Separation: 0.05999999865889549 610 | Stereo Focal Distance: 1 611 | Swap Stereo Eyes: false 612 | Value: false 613 | Field of View: 0.7853981852531433 614 | Focal Point: 615 | X: 73.46565246582031 616 | Y: -55.00351333618164 617 | Z: -20.647184371948242 618 | Focal Shape Fixed Size: false 619 | Focal Shape Size: 0.05000000074505806 620 | Invert Z Axis: false 621 | Name: garden 622 | Near Clip Distance: 0.009999999776482582 623 | Pitch: 1.5597963333129883 624 | Target Frame: 625 | Yaw: 4.355392932891846 626 | - Class: rviz/Orbit 627 | Distance: 440.30224609375 628 | Enable Stereo Rendering: 629 | Stereo Eye Separation: 0.05999999865889549 630 | Stereo Focal Distance: 1 631 | Swap Stereo Eyes: false 632 | Value: false 633 | Field of View: 0.7853981852531433 634 | Focal Point: 635 | X: -30.845176696777344 636 | Y: 166.62318420410156 637 | Z: 129.23764038085938 638 | Focal Shape Fixed Size: false 639 | Focal Shape Size: 0.05000000074505806 640 | Invert Z Axis: false 641 | Name: fly 642 | Near Clip Distance: 0.009999999776482582 643 | Pitch: -0.010202649049460888 644 | Target Frame: 645 | Yaw: 3.255385160446167 646 | - Class: rviz/Orbit 647 | Distance: 93.19469451904297 648 | Enable Stereo Rendering: 649 | Stereo Eye Separation: 0.05999999865889549 650 | Stereo Focal Distance: 1 651 | Swap Stereo Eyes: false 652 | Value: false 653 | Field of View: 0.7853981852531433 654 | Focal Point: 655 | X: 64.74459838867188 656 | Y: -83.73934936523438 657 | Z: 73.07307434082031 658 | Focal Shape Fixed Size: false 659 | Focal Shape Size: 0.05000000074505806 660 | Invert Z Axis: false 661 | Name: park 662 | Near Clip Distance: 0.009999999776482582 663 | Pitch: 1.5697963237762451 664 | Target Frame: 665 | Yaw: 4.00038480758667 666 | - Class: rviz/Orbit 667 | Distance: 806.5628051757812 668 | Enable Stereo Rendering: 669 | Stereo Eye Separation: 0.05999999865889549 670 | Stereo Focal Distance: 1 671 | Swap Stereo Eyes: false 672 | Value: false 673 | Field of View: 0.7853981852531433 674 | Focal Point: 675 | X: 0.03874540328979492 676 | Y: -172.88673400878906 677 | Z: 215.0831756591797 678 | Focal Shape Fixed Size: false 679 | Focal Shape Size: 0.05000000074505806 680 | Invert Z Axis: false 681 | Name: KITTI 682 | Near Clip Distance: 0.009999999776482582 683 | Pitch: 0.010398204438388348 684 | Target Frame: camera_init 685 | Yaw: 1.6403980255126953 686 | - Class: rviz/ThirdPersonFollower 687 | Distance: 19.453998565673828 688 | Enable Stereo Rendering: 689 | Stereo Eye Separation: 0.05999999865889549 690 | Stereo Focal Distance: 1 691 | Swap Stereo Eyes: false 692 | Value: false 693 | Field of View: 0.7853981852531433 694 | Focal Point: 695 | X: -5.738302230834961 696 | Y: 7.975442409515381 697 | Z: 0 698 | Focal Shape Fixed Size: false 699 | Focal Shape Size: 0.05000000074505806 700 | Invert Z Axis: false 701 | Name: Kitti_start 702 | Near Clip Distance: 0.009999999776482582 703 | Pitch: 0.25039851665496826 704 | Target Frame: aft_mapped 705 | Yaw: 3.2403976917266846 706 | Window Geometry: 707 | Displays: 708 | collapsed: false 709 | Height: 1536 710 | Hide Left Dock: false 711 | Hide Right Dock: false 712 | Image: 713 | collapsed: false 714 | QMainWindow State: 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 715 | Selection: 716 | collapsed: false 717 | Time: 718 | collapsed: false 719 | Tool Properties: 720 | collapsed: false 721 | Views: 722 | collapsed: false 723 | Width: 2488 724 | X: 72 725 | Y: 587 726 | -------------------------------------------------------------------------------- /src/IMU_Processing.hpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | /// *************Preconfiguration 28 | #define INITIAL_MEA_NOISE (0.0001) 29 | #define NITIAL_BIAS_NOISE (0.00001) 30 | #define MAX_INI_COUNT (20) 31 | 32 | const bool time_list(PointType& x, PointType& y) { 33 | return (x.curvature < y.curvature); 34 | }; 35 | 36 | /// *************IMU Process and undistortion 37 | class ImuProcess { 38 | public: 39 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 40 | 41 | ImuProcess(); 42 | ~ImuProcess(); 43 | 44 | void Reset(); 45 | void Reset(double start_timestamp, const sensor_msgs::ImuConstPtr& lastimu); 46 | void set_extrinsic(const V3D& transl, const M3D& rot); 47 | void set_extrinsic(const V3D& transl); 48 | void set_extrinsic(const MD(4, 4) & T); 49 | void set_gyr_cov_scale(const V3D& scaler); 50 | void set_acc_cov_scale(const V3D& scaler); 51 | void set_gyr_bias_cov(const V3D& b_g); 52 | void set_acc_bias_cov(const V3D& b_a); 53 | void Process(const MeasureGroup& meas, StatesGroup& state, PointCloudXYZI::Ptr& pcl_un_); 54 | 55 | ros::NodeHandle nh; 56 | V3D cov_acc; 57 | V3D cov_gyr; 58 | V3D cov_acc_scale; 59 | V3D cov_gyr_scale; 60 | V3D cov_bias_gyr; 61 | V3D cov_bias_acc; 62 | M3D Lid_rot_to_IMU; 63 | V3D Lid_offset_to_IMU; 64 | 65 | double first_lidar_time; 66 | bool imu_en; 67 | 68 | private: 69 | void IMU_init(const MeasureGroup& meas, StatesGroup& state, int& N); 70 | void UndistortPcl(const MeasureGroup& meas, StatesGroup& state_inout, PointCloudXYZI& pcl_in_out); 71 | void only_propag(const MeasureGroup& meas, 72 | StatesGroup& state_inout, 73 | PointCloudXYZI::Ptr& pcl_out); 74 | 75 | PointCloudXYZI::Ptr cur_pcl_un_; 76 | sensor_msgs::ImuConstPtr last_imu_; 77 | deque v_imu_; 78 | vector IMUpose; 79 | vector v_rot_pcl_; 80 | 81 | V3D mean_acc; 82 | V3D mean_gyr; 83 | V3D angvel_last; 84 | V3D acc_s_last; 85 | double start_timestamp_; 86 | double time_last_scan_; 87 | int init_iter_num = 1; 88 | bool b_first_frame_ = true; 89 | bool imu_need_init_ = true; 90 | }; 91 | 92 | ImuProcess::ImuProcess() : b_first_frame_(true), imu_need_init_(true), start_timestamp_(-1) { 93 | imu_en = true; 94 | init_iter_num = 1; 95 | cov_acc = V3D(0.1, 0.1, 0.1); 96 | cov_gyr = V3D(0.1, 0.1, 0.1); 97 | // old 98 | cov_acc_scale = V3D(1, 1, 1); 99 | cov_gyr_scale = V3D(1, 1, 1); 100 | 101 | cov_bias_gyr = V3D(0.1, 0.1, 0.1); 102 | cov_bias_acc = V3D(0.1, 0.1, 0.1); 103 | mean_acc = V3D(0, 0, -1.0); 104 | mean_gyr = V3D(0, 0, 0); 105 | angvel_last = Zero3d; 106 | Lid_offset_to_IMU = Zero3d; 107 | Lid_rot_to_IMU = Eye3d; 108 | last_imu_.reset(new sensor_msgs::Imu()); 109 | } 110 | 111 | ImuProcess::~ImuProcess() {} 112 | 113 | void ImuProcess::Reset() { 114 | ROS_WARN("Reset ImuProcess"); 115 | mean_acc = V3D(0, 0, -1.0); 116 | mean_gyr = V3D(0, 0, 0); 117 | angvel_last = Zero3d; 118 | imu_need_init_ = true; 119 | start_timestamp_ = -1; 120 | init_iter_num = 1; 121 | v_imu_.clear(); 122 | IMUpose.clear(); 123 | last_imu_.reset(new sensor_msgs::Imu()); 124 | cur_pcl_un_.reset(new PointCloudXYZI()); 125 | } 126 | 127 | void ImuProcess::set_extrinsic(const MD(4, 4) & T) { 128 | Lid_offset_to_IMU = T.block<3, 1>(0, 3); 129 | Lid_rot_to_IMU = T.block<3, 3>(0, 0); 130 | } 131 | 132 | void ImuProcess::set_extrinsic(const V3D& transl) { 133 | Lid_offset_to_IMU = transl; 134 | Lid_rot_to_IMU.setIdentity(); 135 | } 136 | 137 | void ImuProcess::set_extrinsic(const V3D& transl, const M3D& rot) { 138 | Lid_offset_to_IMU = transl; 139 | Lid_rot_to_IMU = rot; 140 | } 141 | 142 | void ImuProcess::set_gyr_cov_scale(const V3D& scaler) { 143 | cov_gyr_scale = scaler; 144 | } 145 | 146 | void ImuProcess::set_acc_cov_scale(const V3D& scaler) { 147 | cov_acc_scale = scaler; 148 | } 149 | 150 | void ImuProcess::set_gyr_bias_cov(const V3D& b_g) { 151 | cov_bias_gyr = b_g; 152 | } 153 | 154 | void ImuProcess::set_acc_bias_cov(const V3D& b_a) { 155 | cov_bias_acc = b_a; 156 | } 157 | 158 | void ImuProcess::IMU_init(const MeasureGroup& meas, StatesGroup& state_inout, int& N) { 159 | /** 1. initializing the gravity, gyro bias, acc and gyro covariance 160 | ** 2. normalize the acceleration measurenments to unit gravity **/ 161 | V3D cur_acc, cur_gyr; 162 | 163 | if (b_first_frame_) { 164 | Reset(); 165 | N = 1; 166 | b_first_frame_ = false; 167 | const auto& imu_acc = meas.imu.front()->linear_acceleration; 168 | const auto& gyr_acc = meas.imu.front()->angular_velocity; 169 | mean_acc << imu_acc.x, imu_acc.y, imu_acc.z; 170 | mean_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z; 171 | first_lidar_time = meas.lidar_beg_time; 172 | // cout<<"init acc norm: "<linear_acceleration; 177 | const auto& gyr_acc = imu->angular_velocity; 178 | cur_acc << imu_acc.x, imu_acc.y, imu_acc.z; 179 | cur_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z; 180 | 181 | mean_acc += (cur_acc - mean_acc) / N; 182 | mean_gyr += (cur_gyr - mean_gyr) / N; 183 | 184 | cov_acc = cov_acc * (N - 1.0) / N + 185 | (cur_acc - mean_acc).cwiseProduct(cur_acc - mean_acc) * (N - 1.0) / (N * N); 186 | cov_gyr = cov_gyr * (N - 1.0) / N + 187 | (cur_gyr - mean_gyr).cwiseProduct(cur_gyr - mean_gyr) * (N - 1.0) / (N * N); 188 | 189 | // cout<<"acc norm: "<header.stamp.toSec(); 209 | const double& imu_end_time = v_imu.back()->header.stamp.toSec(); 210 | const double& pcl_beg_time = meas.lidar_beg_time; 211 | 212 | /*** sort point clouds by offset time ***/ 213 | pcl_out = *(meas.lidar); 214 | sort(pcl_out.points.begin(), pcl_out.points.end(), time_list); 215 | const double& pcl_end_time = pcl_beg_time + pcl_out.points.back().curvature / double(1000); 216 | // cout<<"[ IMU Process ]: Process lidar from "<header.stamp.toSec() < time_last_scan_) 240 | continue; 241 | 242 | angvel_avr << 0.5 * (head->angular_velocity.x + tail->angular_velocity.x), 243 | 0.5 * (head->angular_velocity.y + tail->angular_velocity.y), 244 | 0.5 * (head->angular_velocity.z + tail->angular_velocity.z); 245 | 246 | acc_avr << 0.5 * (head->linear_acceleration.x + tail->linear_acceleration.x), 247 | 0.5 * (head->linear_acceleration.y + tail->linear_acceleration.y), 248 | 0.5 * (head->linear_acceleration.z + tail->linear_acceleration.z); 249 | 250 | angvel_avr -= state_inout.bias_g; 251 | acc_avr = acc_avr * G_m_s2 / mean_acc.norm() - state_inout.bias_a; 252 | 253 | if (head->header.stamp.toSec() < time_last_scan_) { 254 | dt = tail->header.stamp.toSec() - time_last_scan_; 255 | } else { 256 | dt = tail->header.stamp.toSec() - head->header.stamp.toSec(); 257 | } 258 | 259 | /* covariance propagation */ 260 | M3D acc_avr_skew; 261 | M3D Exp_f = Exp(angvel_avr, dt); 262 | acc_avr_skew << SKEW_SYM_MATRX(acc_avr); 263 | 264 | F_x.setIdentity(); 265 | cov_w.setZero(); 266 | 267 | F_x.block<3, 3>(0, 0) = Exp(angvel_avr, -dt); 268 | F_x.block<3, 3>(0, 9) = -Eye3d * dt; 269 | // F_x.block<3,3>(3,0) = R_imu * off_vel_skew * dt; 270 | F_x.block<3, 3>(3, 6) = Eye3d * dt; 271 | F_x.block<3, 3>(6, 0) = -R_imu * acc_avr_skew * dt; 272 | F_x.block<3, 3>(6, 12) = -R_imu * dt; 273 | F_x.block<3, 3>(6, 15) = Eye3d * dt; 274 | 275 | cov_w.block<3, 3>(0, 0).diagonal() = cov_gyr * dt * dt; 276 | cov_w.block<3, 3>(6, 6) = R_imu * cov_acc.asDiagonal() * R_imu.transpose() * dt * dt; 277 | cov_w.block<3, 3>(9, 9).diagonal() = cov_bias_gyr * dt * dt; // bias gyro covariance 278 | cov_w.block<3, 3>(12, 12).diagonal() = cov_bias_acc * dt * dt; // bias acc covariance 279 | 280 | state_inout.cov = F_x * state_inout.cov * F_x.transpose() + cov_w; 281 | 282 | /* propogation of IMU attitude */ 283 | R_imu = R_imu * Exp_f; 284 | 285 | /* Specific acceleration (global frame) of IMU */ 286 | acc_imu = R_imu * acc_avr + state_inout.gravity; 287 | 288 | /* propogation of IMU */ 289 | pos_imu = pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt; 290 | 291 | /* velocity of IMU */ 292 | vel_imu = vel_imu + acc_imu * dt; 293 | 294 | /* save the poses at each IMU measurements */ 295 | angvel_last = angvel_avr; 296 | acc_s_last = acc_imu; 297 | double&& offs_t = tail->header.stamp.toSec() - pcl_beg_time; 298 | 299 | IMUpose.push_back(set_pose6d(offs_t, acc_imu, angvel_avr, vel_imu, pos_imu, R_imu)); 300 | } 301 | 302 | /*** calculated the pos and attitude prediction at the frame-end ***/ 303 | if (imu_end_time > pcl_beg_time) { 304 | double note = pcl_end_time > imu_end_time ? 1.0 : -1.0; 305 | dt = note * (pcl_end_time - imu_end_time); 306 | state_inout.vel_end = vel_imu + note * acc_imu * dt; 307 | state_inout.rot_end = R_imu * Exp(V3D(note * angvel_avr), dt); 308 | state_inout.pos_end = pos_imu + note * vel_imu * dt + note * 0.5 * acc_imu * dt * dt; 309 | } else { 310 | double note = pcl_end_time > pcl_beg_time ? 1.0 : -1.0; 311 | dt = note * (pcl_end_time - pcl_beg_time); 312 | state_inout.vel_end = vel_imu + note * acc_imu * dt; 313 | state_inout.rot_end = R_imu * Exp(V3D(note * angvel_avr), dt); 314 | state_inout.pos_end = pos_imu + note * vel_imu * dt + note * 0.5 * acc_imu * dt * dt; 315 | } 316 | 317 | last_imu_ = v_imu.back(); 318 | time_last_scan_ = pcl_end_time; 319 | 320 | if (pcl_out.points.empty()) { 321 | return; 322 | } 323 | 324 | /*** undistort each lidar point (backward propagation) ***/ 325 | auto it_pcl = pcl_out.points.end() - 1; 326 | 327 | for (auto it_kp = IMUpose.end() - 1; it_kp != IMUpose.begin(); it_kp--) { 328 | auto head = it_kp - 1; 329 | auto tail = it_kp; 330 | R_imu << MAT_FROM_ARRAY(head->rot); 331 | acc_imu << VEC_FROM_ARRAY(head->acc); 332 | 333 | vel_imu << VEC_FROM_ARRAY(head->vel); 334 | pos_imu << VEC_FROM_ARRAY(head->pos); 335 | angvel_avr << VEC_FROM_ARRAY(head->gyr); 336 | 337 | for (; it_pcl->curvature / double(1000) > head->offset_time; it_pcl--) { 338 | dt = it_pcl->curvature / double(1000) - head->offset_time; 339 | 340 | // /* Transform to the 'end' frame, using only the rotation 341 | // * Note: Compensation direction is INVERSE of Frame's moving direction 342 | // * So if we want to compensate a point at timestamp-i to the frame-e 343 | // * P_compensate = R_imu_e ^ T * (R_i * P_i + T_ei) where T_ei is 344 | // * represented in global frame */ 345 | M3D R_i(R_imu * Exp(angvel_avr, dt)); 346 | V3D T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt - state_inout.pos_end); 347 | 348 | V3D P_i(it_pcl->x, it_pcl->y, it_pcl->z); 349 | V3D P_compensate = Lid_rot_to_IMU.transpose() * 350 | (state_inout.rot_end.transpose() * 351 | (R_i * (Lid_rot_to_IMU * P_i + Lid_offset_to_IMU) + T_ei) - 352 | Lid_offset_to_IMU); 353 | // save Undistorted points and their rotation 354 | it_pcl->x = P_compensate(0); 355 | it_pcl->y = P_compensate(1); 356 | it_pcl->z = P_compensate(2); 357 | 358 | if (it_pcl == pcl_out.points.begin()) 359 | break; 360 | } 361 | } 362 | } 363 | 364 | // constant velocity model 365 | void ImuProcess::only_propag(const MeasureGroup& meas, 366 | StatesGroup& state_inout, 367 | PointCloudXYZI::Ptr& pcl_out) { 368 | const double& pcl_beg_time = meas.lidar_beg_time; 369 | 370 | /*** sort point clouds by offset time ***/ 371 | pcl_out = meas.lidar; 372 | const double& pcl_end_time = pcl_beg_time + pcl_out->points.back().curvature / double(1000); 373 | 374 | MD(DIM_STATE, DIM_STATE) F_x, cov_w; 375 | double dt = 0; 376 | 377 | if (b_first_frame_) { 378 | dt = 0.1; 379 | b_first_frame_ = false; 380 | time_last_scan_ = pcl_beg_time; 381 | } else { 382 | dt = pcl_beg_time - time_last_scan_; 383 | time_last_scan_ = pcl_beg_time; 384 | } 385 | 386 | /* covariance propagation */ 387 | // M3D acc_avr_skew; 388 | M3D Exp_f = Exp(state_inout.bias_g, dt); 389 | 390 | F_x.setIdentity(); 391 | cov_w.setZero(); 392 | 393 | F_x.block<3, 3>(0, 0) = Exp(state_inout.bias_g, -dt); 394 | F_x.block<3, 3>(0, 9) = Eye3d * dt; 395 | F_x.block<3, 3>(3, 6) = Eye3d * dt; 396 | cov_w.block<3, 3>(9, 9).diagonal() = cov_gyr * dt * dt; // for omega in constant model 397 | cov_w.block<3, 3>(6, 6).diagonal() = cov_acc * dt * dt; // for velocity in constant model 398 | 399 | state_inout.cov = F_x * state_inout.cov * F_x.transpose() + cov_w; 400 | state_inout.rot_end = state_inout.rot_end * Exp_f; 401 | state_inout.pos_end = state_inout.pos_end + state_inout.vel_end * dt; 402 | } 403 | 404 | void ImuProcess::Process(const MeasureGroup& meas, 405 | StatesGroup& stat, 406 | PointCloudXYZI::Ptr& cur_pcl_un_) { 407 | double t1, t2, t3; 408 | 409 | if (meas.imu.empty() && imu_en) { 410 | return; 411 | } 412 | ROS_ASSERT(meas.lidar != nullptr); 413 | 414 | if (imu_need_init_ && imu_en) { 415 | /// The very first lidar frame 416 | IMU_init(meas, stat, init_iter_num); 417 | 418 | imu_need_init_ = true; 419 | 420 | last_imu_ = meas.imu.back(); 421 | 422 | if (init_iter_num > MAX_INI_COUNT) { 423 | imu_need_init_ = false; 424 | 425 | cov_acc = cov_acc_scale; 426 | cov_gyr = cov_gyr_scale; 427 | 428 | ROS_INFO("IMU Initial Done!"); 429 | ROS_INFO("Gravity: %.4f %.4f %.4f %.4f; state.bias_g: %.8f " 430 | "%.8f %.8f; acc covarience: %.8f %.8f %.8f; gry covarience: " 431 | "%.8f %.8f %.8f", 432 | stat.gravity[0], 433 | stat.gravity[1], 434 | stat.gravity[2], 435 | mean_acc.norm(), 436 | cov_bias_gyr[0], 437 | cov_bias_gyr[1], 438 | cov_bias_gyr[2], 439 | cov_acc[0], 440 | cov_acc[1], 441 | cov_acc[2], 442 | cov_gyr[0], 443 | cov_gyr[1], 444 | cov_gyr[2]); 445 | } 446 | 447 | return; 448 | } 449 | 450 | /// Undistort points： the first point is assummed as the base frame 451 | /// Compensate lidar points with IMU rotation (with only rotation now) 452 | if (imu_en) { 453 | // cout << "Use IMU" << endl; 454 | UndistortPcl(meas, stat, *cur_pcl_un_); 455 | last_imu_ = meas.imu.back(); 456 | } else { 457 | // cout << "No IMU, use constant velocity model" << endl; 458 | cov_acc = Eye3d * cov_acc_scale; 459 | cov_gyr = Eye3d * cov_gyr_scale; 460 | only_propag(meas, stat, cur_pcl_un_); 461 | } 462 | } -------------------------------------------------------------------------------- /src/preprocess.cpp: -------------------------------------------------------------------------------- 1 | #include "preprocess.h" 2 | 3 | #define RETURN0 0x00 4 | #define RETURN0AND1 0x10 5 | 6 | Preprocess::Preprocess() : feature_enabled(0), lidar_type(AVIA), blind(0.01), point_filter_num(1) { 7 | N_SCANS = 6; 8 | given_offset_time = false; 9 | } 10 | 11 | Preprocess::~Preprocess() {} 12 | 13 | void Preprocess::set(bool feat_en, int lid_type, double bld, int pfilt_num) { 14 | feature_enabled = feat_en; 15 | lidar_type = lid_type; 16 | blind = bld; 17 | point_filter_num = pfilt_num; 18 | } 19 | 20 | void Preprocess::process(const livox_ros_driver::CustomMsg::ConstPtr& msg, 21 | PointCloudXYZI::Ptr& pcl_out) { 22 | avia_handler(msg); 23 | *pcl_out = pl_surf; 24 | } 25 | 26 | void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr& msg, 27 | PointCloudXYZI::Ptr& pcl_out) { 28 | switch (lidar_type) { 29 | case L515: 30 | l515_handler(msg); 31 | break; 32 | 33 | case VELO16: 34 | velodyne_handler(msg); 35 | break; 36 | 37 | case VELO32: 38 | velodyne_handler32(msg); 39 | break; 40 | 41 | default: 42 | printf("Error LiDAR Type"); 43 | break; 44 | } 45 | *pcl_out = pl_surf; 46 | } 47 | 48 | void Preprocess::avia_handler(const livox_ros_driver::CustomMsg::ConstPtr& msg) { 49 | pl_surf.clear(); 50 | pl_corn.clear(); 51 | pl_full.clear(); 52 | int plsize = msg->point_num; 53 | std::vector is_valid_pt(plsize, false); 54 | 55 | pl_corn.reserve(plsize); 56 | pl_surf.reserve(plsize); 57 | pl_full.resize(plsize); 58 | 59 | for (int i = 0; i < N_SCANS; i++) { 60 | pl_buff[i].clear(); 61 | pl_buff[i].reserve(plsize); 62 | } 63 | uint valid_num = 0; 64 | 65 | for (uint i = 1; i < plsize; i++) { 66 | if ((msg->points[i].line < N_SCANS) && 67 | ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00)) { 68 | valid_num++; 69 | if (i % point_filter_num == 0) { 70 | pl_full[i].x = msg->points[i].x; 71 | pl_full[i].y = msg->points[i].y; 72 | pl_full[i].z = msg->points[i].z; 73 | pl_full[i].intensity = msg->points[i].reflectivity; 74 | pl_full[i].curvature = msg->points[i].offset_time / 75 | float(1000000); // use curvature as time of each laser points 76 | 77 | if ((abs(pl_full[i].x - pl_full[i - 1].x) > 1e-7) || 78 | (abs(pl_full[i].y - pl_full[i - 1].y) > 1e-7) || 79 | (abs(pl_full[i].z - pl_full[i - 1].z) > 1e-7) && 80 | (pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y + pl_full[i].z + 81 | pl_full[i].z > 82 | blind * blind)) { 83 | is_valid_pt[i] = true; 84 | } 85 | } 86 | } 87 | } 88 | 89 | for (uint i = 1; i < plsize; i++) { 90 | if (is_valid_pt[i]) { 91 | pl_surf.points.push_back(pl_full[i]); 92 | } 93 | } 94 | } 95 | 96 | void Preprocess::oust64_handler(const sensor_msgs::PointCloud2::ConstPtr& msg) { 97 | pl_surf.clear(); 98 | pl_corn.clear(); 99 | pl_full.clear(); 100 | pcl::PointCloud pl_orig; 101 | pcl::fromROSMsg(*msg, pl_orig); 102 | int plsize = pl_orig.size(); 103 | pl_corn.reserve(plsize); 104 | pl_surf.reserve(plsize); 105 | 106 | double time_stamp = msg->header.stamp.toSec(); 107 | // cout << "===================================" << endl; 108 | // printf("Pt size = %d, N_SCANS = %d\r\n", plsize, N_SCANS); 109 | for (int i = 0; i < pl_orig.points.size(); i++) { 110 | if (i % point_filter_num != 0) 111 | continue; 112 | 113 | double range = pl_orig.points[i].x * pl_orig.points[i].x + 114 | pl_orig.points[i].y * pl_orig.points[i].y + 115 | pl_orig.points[i].z * pl_orig.points[i].z; 116 | 117 | if (range < blind) 118 | continue; 119 | 120 | Eigen::Vector3d pt_vec; 121 | PointType added_pt; 122 | added_pt.x = pl_orig.points[i].x; 123 | added_pt.y = pl_orig.points[i].y; 124 | added_pt.z = pl_orig.points[i].z; 125 | added_pt.intensity = pl_orig.points[i].intensity; 126 | added_pt.normal_x = 0; 127 | added_pt.normal_y = 0; 128 | added_pt.normal_z = 0; 129 | double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.3; 130 | if (yaw_angle >= 180.0) 131 | yaw_angle -= 360.0; 132 | if (yaw_angle <= -180.0) 133 | yaw_angle += 360.0; 134 | 135 | added_pt.curvature = pl_orig.points[i].t / 1e6; 136 | 137 | pl_surf.points.push_back(added_pt); 138 | } 139 | } 140 | 141 | void Preprocess::l515_handler(const sensor_msgs::PointCloud2::ConstPtr& msg) { 142 | pl_surf.clear(); 143 | pcl::PointCloud pl_orig; 144 | pcl::fromROSMsg(*msg, pl_orig); 145 | int plsize = pl_orig.points.size(); 146 | // pl_surf.reserve(plsize); 147 | for (int i = 0; i < pl_orig.size(); i++) { 148 | PointType added_pt; 149 | added_pt.x = pl_orig.points[i].x; 150 | added_pt.y = pl_orig.points[i].y; 151 | added_pt.z = pl_orig.points[i].z; 152 | added_pt.intensity = pl_orig.points[i].intensity; 153 | double range = pl_orig.points[i].x * pl_orig.points[i].x + 154 | pl_orig.points[i].y * pl_orig.points[i].y + 155 | pl_orig.points[i].z * pl_orig.points[i].z; 156 | 157 | if (range < blind) 158 | continue; 159 | 160 | if (i % point_filter_num == 0) { 161 | pl_surf.push_back(added_pt); 162 | } 163 | } 164 | } 165 | 166 | #define MAX_LINE_NUM 64 167 | 168 | void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr& msg) { 169 | pl_surf.clear(); 170 | pl_corn.clear(); 171 | pl_full.clear(); 172 | 173 | pcl::PointCloud pl_orig; 174 | pcl::fromROSMsg(*msg, pl_orig); 175 | int plsize = pl_orig.points.size(); 176 | // pl_surf.reserve(plsize); 177 | for (int i = 0; i < pl_orig.size(); i++) { 178 | PointType added_pt; 179 | added_pt.x = pl_orig.points[i].x; 180 | added_pt.y = pl_orig.points[i].y; 181 | added_pt.z = pl_orig.points[i].z; 182 | added_pt.intensity = pl_orig.points[i].intensity; 183 | float angle = atan(added_pt.z / sqrt(added_pt.x * added_pt.x + added_pt.y * added_pt.y)) * 180 / 184 | M_PI; 185 | int scanID = 0; 186 | if (angle >= -8.83) 187 | scanID = int((2 - angle) * 3.0 + 0.5); 188 | else 189 | scanID = N_SCANS / 2 + int((-8.83 - angle) * 2.0 + 0.5); 190 | 191 | // use [0 50] > 50 remove outlies 192 | if (angle > 2 || angle < -24.33 || scanID > 50 || scanID < 0) { 193 | continue; 194 | } 195 | pl_surf.push_back(added_pt); 196 | } 197 | } 198 | 199 | void Preprocess::velodyne_handler32(const sensor_msgs::PointCloud2::ConstPtr& msg) { 200 | pl_surf.clear(); 201 | pl_corn.clear(); 202 | pl_full.clear(); 203 | 204 | pcl::PointCloud pl_orig; 205 | pcl::fromROSMsg(*msg, pl_orig); 206 | int plsize = pl_orig.points.size(); 207 | 208 | /*** These variables only works when no point timestamps given ***/ 209 | double omega_l = 3.61; // scan angular velocity 210 | std::vector is_first(N_SCANS, true); 211 | std::vector yaw_fp(N_SCANS, 0.0); // yaw of first scan point 212 | std::vector yaw_last(N_SCANS, 0.0); // yaw of last scan point 213 | std::vector time_last(N_SCANS, 0.0); // last offset time 214 | /*****************************************************************/ 215 | 216 | if (pl_orig.points[plsize - 1].time > 0) { 217 | given_offset_time = true; 218 | } else { 219 | given_offset_time = false; 220 | double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578; 221 | double yaw_end = yaw_first; 222 | int layer_first = pl_orig.points[0].ring; 223 | for (uint i = plsize - 1; i > 0; i--) { 224 | if (pl_orig.points[i].ring == layer_first) { 225 | yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578; 226 | break; 227 | } 228 | } 229 | } 230 | 231 | for (int i = 0; i < plsize; i++) { 232 | PointType added_pt; 233 | 234 | added_pt.normal_x = 0; 235 | added_pt.normal_y = 0; 236 | added_pt.normal_z = 0; 237 | added_pt.x = pl_orig.points[i].x; 238 | added_pt.y = pl_orig.points[i].y; 239 | added_pt.z = pl_orig.points[i].z; 240 | added_pt.intensity = pl_orig.points[i].intensity; 241 | added_pt.curvature = pl_orig.points[i].time * 1e-3; // curvature unit: ms 242 | 243 | if (!given_offset_time) { 244 | int layer = pl_orig.points[i].ring; 245 | double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957; 246 | 247 | if (is_first[layer]) { 248 | yaw_fp[layer] = yaw_angle; 249 | is_first[layer] = false; 250 | added_pt.curvature = 0.0; 251 | yaw_last[layer] = yaw_angle; 252 | time_last[layer] = added_pt.curvature; 253 | continue; 254 | } 255 | 256 | // compute offset time 257 | if (yaw_angle <= yaw_fp[layer]) { 258 | added_pt.curvature = (yaw_fp[layer] - yaw_angle) / omega_l; 259 | } else { 260 | added_pt.curvature = (yaw_fp[layer] - yaw_angle + 360.0) / omega_l; 261 | } 262 | 263 | if (added_pt.curvature < time_last[layer]) 264 | added_pt.curvature += 360.0 / omega_l; 265 | 266 | yaw_last[layer] = yaw_angle; 267 | time_last[layer] = added_pt.curvature; 268 | } 269 | 270 | if (i % point_filter_num == 0) { 271 | if (added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z > 272 | (blind * blind)) { 273 | pl_surf.push_back(added_pt); 274 | } 275 | } 276 | } 277 | } 278 | -------------------------------------------------------------------------------- /src/preprocess.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | using namespace std; 7 | 8 | #define IS_VALID(a) ((abs(a) > 1e8) ? true : false) 9 | 10 | typedef pcl::PointXYZINormal PointType; 11 | typedef pcl::PointCloud PointCloudXYZI; 12 | 13 | enum LID_TYPE { AVIA = 1, VELO16, L515, OUSTER64, VELO32 }; //{1, 2, 3} 14 | 15 | namespace velodyne_ros { 16 | struct EIGEN_ALIGN16 Point { 17 | PCL_ADD_POINT4D; 18 | float intensity; 19 | float time; 20 | uint16_t ring; 21 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 22 | }; 23 | } // namespace velodyne_ros 24 | POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point, 25 | (float, x, x)(float, y, y)(float, z, z)( 26 | float, 27 | intensity, 28 | intensity)(float, time, time)(std::uint16_t, ring, ring)) 29 | 30 | namespace ouster_ros { 31 | struct EIGEN_ALIGN16 Point { 32 | PCL_ADD_POINT4D; 33 | float intensity; 34 | uint32_t t; 35 | uint16_t reflectivity; 36 | uint8_t ring; 37 | uint16_t ambient; 38 | uint32_t range; 39 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 40 | }; 41 | } // namespace ouster_ros 42 | 43 | // clang-format off 44 | POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point, 45 | (float, x, x) 46 | (float, y, y) 47 | (float, z, z) 48 | (float, intensity, intensity) 49 | // use std::uint32_t to avoid conflicting with pcl::uint32_t 50 | (std::uint32_t, t, t) 51 | (std::uint16_t, reflectivity, reflectivity) 52 | (std::uint8_t, ring, ring) 53 | (std::uint16_t, ambient, ambient) 54 | (std::uint32_t, range, range) 55 | ) 56 | 57 | class Preprocess 58 | { 59 | public: 60 | // EIGEN_MAKE_ALIGNED_OPERATOR_NEW 61 | 62 | Preprocess(); 63 | ~Preprocess(); 64 | 65 | void process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out); 66 | void process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out); 67 | void set(bool feat_en, int lid_type, double bld, int pfilt_num); 68 | 69 | // sensor_msgs::PointCloud2::ConstPtr pointcloud; 70 | PointCloudXYZI pl_full, pl_corn, pl_surf; 71 | PointCloudXYZI pl_buff[128]; //maximum 128 line lidar 72 | int lidar_type, point_filter_num, N_SCANS;; 73 | double blind; 74 | bool feature_enabled, given_offset_time; 75 | ros::Publisher pub_full, pub_surf, pub_corn; 76 | 77 | 78 | private: 79 | void avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg); 80 | void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); 81 | void l515_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); 82 | void velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); 83 | void velodyne_handler32(const sensor_msgs::PointCloud2::ConstPtr& msg); 84 | 85 | }; 86 | -------------------------------------------------------------------------------- /src/voxelMapping.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include "IMU_Processing.hpp" 30 | #include "preprocess.h" 31 | #include "voxel_map_util.hpp" 32 | 33 | #define INIT_TIME (0.0) 34 | #define CALIB_ANGLE_COV (0.01) 35 | bool calib_laser = false; 36 | bool write_kitti_log = false; 37 | std::string result_path = ""; 38 | // params for imu 39 | bool imu_en = true; 40 | std::vector extrinT; 41 | std::vector extrinR; 42 | 43 | // params for publish function 44 | bool publish_voxel_map = false; 45 | int publish_max_voxel_layer = 0; 46 | bool publish_point_cloud = false; 47 | int pub_point_cloud_skip = 1; 48 | 49 | double intensity_min_thr = 0.0, intensity_max_thr = 1.0; 50 | 51 | // params for voxel mapping algorithm 52 | double min_eigen_value = 0.003; 53 | int max_layer = 0; 54 | 55 | double sigma_num = 2.0; 56 | double max_voxel_size = 1.0; 57 | std::vector layer_size; 58 | // Eigen::Vector3d layer_size(20, 10, 10); 59 | // avia 60 | // velodyne 61 | // Eigen::Vector3d layer_size(10, 6, 6); 62 | 63 | // record point usage 64 | double mean_effect_points = 0; 65 | double mean_ds_points = 0; 66 | double mean_raw_points = 0; 67 | 68 | // record time 69 | double undistort_time_mean = 0; 70 | double down_sample_time_mean = 0; 71 | double calc_cov_time_mean = 0; 72 | double scan_match_time_mean = 0; 73 | double ekf_solve_time_mean = 0; 74 | double map_update_time_mean = 0; 75 | 76 | mutex mtx_buffer; 77 | condition_variable sig_buffer; 78 | Eigen::Vector3d last_odom(0, 0, 0); 79 | Eigen::Matrix3d last_rot = Eigen::Matrix3d::Zero(); 80 | double trajectory_len = 0; 81 | 82 | string map_file_path, lid_topic, imu_topic; 83 | int scanIdx = 0; 84 | 85 | int iterCount, feats_down_size, NUM_MAX_ITERATIONS, laserCloudValidNum, effct_feat_num, 86 | time_log_counter, publish_count = 0; 87 | 88 | double first_lidar_time = 0; 89 | double lidar_end_time = 0; 90 | double res_mean_last = 0.05; 91 | double total_distance = 0; 92 | double gyr_cov_scale, acc_cov_scale; 93 | double last_timestamp_lidar, last_timestamp_imu = -1.0; 94 | double filter_size_corner_min, filter_size_surf_min, fov_deg; 95 | double map_incremental_time, kdtree_search_time, total_time, scan_match_time, solve_time; 96 | bool lidar_pushed, flg_reset, flg_exit = false; 97 | bool dense_map_en = true; 98 | 99 | deque lidar_buffer; 100 | deque time_buffer; 101 | deque imu_buffer; 102 | 103 | // surf feature in map 104 | PointCloudXYZI::Ptr featsFromMap(new PointCloudXYZI()); 105 | PointCloudXYZI::Ptr cube_points_add(new PointCloudXYZI()); 106 | PointCloudXYZI::Ptr feats_undistort(new PointCloudXYZI()); 107 | PointCloudXYZI::Ptr feats_down_body(new PointCloudXYZI()); 108 | PointCloudXYZI::Ptr feats_down_world(new PointCloudXYZI()); 109 | PointCloudXYZI::Ptr map_down_body(new PointCloudXYZI()); 110 | PointCloudXYZI::Ptr normvec(new PointCloudXYZI(100000, 1)); 111 | PointCloudXYZI::Ptr laserCloudOri(new PointCloudXYZI(100000, 1)); 112 | PointCloudXYZI::Ptr laserCloudNoeffect(new PointCloudXYZI(100000, 1)); 113 | pcl::VoxelGrid downSizeFilterSurf; 114 | pcl::VoxelGrid downSizeFilterMap; 115 | 116 | V3D euler_cur; 117 | V3D position_last(Zero3d); 118 | 119 | // estimator inputs and output; 120 | MeasureGroup Measures; 121 | StatesGroup state; 122 | 123 | nav_msgs::Path path; 124 | nav_msgs::Odometry odomAftMapped; 125 | geometry_msgs::Quaternion geoQuat; 126 | geometry_msgs::PoseStamped msg_body_pose; 127 | 128 | shared_ptr p_pre(new Preprocess()); 129 | 130 | void SigHandle(int sig) { 131 | flg_exit = true; 132 | ROS_WARN("catch sig %d", sig); 133 | sig_buffer.notify_all(); 134 | } 135 | 136 | const bool intensity_contrast(PointType& x, PointType& y) { 137 | return (x.intensity > y.intensity); 138 | }; 139 | 140 | const bool var_contrast(pointWithCov& x, pointWithCov& y) { 141 | return (x.cov.diagonal().norm() < y.cov.diagonal().norm()); 142 | }; 143 | 144 | inline void dump_lio_state_to_log(FILE* fp) { 145 | V3D rot_ang(Log(state.rot_end)); 146 | fprintf(fp, "%lf ", Measures.lidar_beg_time - first_lidar_time); 147 | fprintf(fp, "%lf %lf %lf ", rot_ang(0), rot_ang(1), rot_ang(2)); // Angle 148 | fprintf(fp, "%lf %lf %lf ", state.pos_end(0), state.pos_end(1), 149 | state.pos_end(2)); // Pos 150 | fprintf(fp, "%lf %lf %lf ", state.vel_end(0), state.vel_end(1), 151 | state.vel_end(2)); // Vel 152 | fprintf(fp, "%lf %lf %lf ", state.bias_g(0), state.bias_g(1), 153 | state.bias_g(2)); // omega 154 | fprintf(fp, 155 | "%lf %lf %lf %lf ", 156 | scan_match_time, 157 | solve_time, 158 | map_incremental_time, 159 | total_time); // scan match, ekf, map incre, total 160 | fprintf(fp, 161 | "%lu %lu %d", 162 | feats_undistort->points.size(), 163 | feats_down_body->points.size(), 164 | effct_feat_num); // raw point number, effect number 165 | fprintf(fp, "\r\n"); 166 | fflush(fp); 167 | } 168 | 169 | inline void kitti_log(FILE* fp) { 170 | Eigen::Matrix4d T_lidar_to_cam; 171 | T_lidar_to_cam << 0.00042768, -0.999967, -0.0080845, -0.01198, -0.00721062, 0.0080811998, 172 | -0.99994131, -0.0540398, 0.999973864, 0.00048594, -0.0072069, -0.292196, 0, 0, 0, 1.0; 173 | V3D rot_ang(Log(state.rot_end)); 174 | MD(4, 4) T; 175 | T.block<3, 3>(0, 0) = state.rot_end; 176 | T.block<3, 1>(0, 3) = state.pos_end; 177 | T(3, 0) = 0; 178 | T(3, 1) = 0; 179 | T(3, 2) = 0; 180 | T(3, 3) = 1; 181 | T = T_lidar_to_cam * T * T_lidar_to_cam.inverse(); 182 | for (int i = 0; i < 3; i++) { 183 | if (i == 2) 184 | fprintf(fp, "%lf %lf %lf %lf", T(i, 0), T(i, 1), T(i, 2), T(i, 3)); 185 | else 186 | fprintf(fp, "%lf %lf %lf %lf ", T(i, 0), T(i, 1), T(i, 2), T(i, 3)); 187 | } 188 | fprintf(fp, "\n"); 189 | // Eigen::Quaterniond q(state.rot_end); 190 | // fprintf(fp, "%lf %lf %lf %lf %lf %lf %lf \r\n", state.pos_end[0], 191 | // state.pos_end[1], state.pos_end[2], q.w(), q.x(), q.y(), q.z()); 192 | fflush(fp); 193 | } 194 | 195 | // project the lidar scan to world frame 196 | void pointBodyToWorld(PointType const* const pi, PointType* const po) { 197 | V3D p_body(pi->x, pi->y, pi->z); 198 | p_body = p_body + Lidar_offset_to_IMU; 199 | V3D p_global(state.rot_end * (p_body) + state.pos_end); 200 | po->x = p_global(0); 201 | po->y = p_global(1); 202 | po->z = p_global(2); 203 | po->intensity = pi->intensity; 204 | } 205 | 206 | template 207 | void pointBodyToWorld(const Matrix& pi, Matrix& po) { 208 | V3D p_body(pi[0], pi[1], pi[2]); 209 | p_body = p_body + Lidar_offset_to_IMU; 210 | V3D p_global(state.rot_end * (p_body) + state.pos_end); 211 | po[0] = p_global(0); 212 | po[1] = p_global(1); 213 | po[2] = p_global(2); 214 | } 215 | 216 | void RGBpointBodyToWorld(PointType const* const pi, PointType* const po) { 217 | V3D p_body(pi->x, pi->y, pi->z); 218 | V3D p_global(state.rot_end * (p_body) + state.pos_end); 219 | po->x = p_global(0); 220 | po->y = p_global(1); 221 | po->z = p_global(2); 222 | po->intensity = pi->intensity; 223 | po->curvature = pi->curvature; 224 | po->normal_x = pi->normal_x; 225 | po->normal_y = pi->normal_y; 226 | po->normal_z = pi->normal_z; 227 | float intensity = pi->intensity; 228 | intensity = intensity - floor(intensity); 229 | 230 | int reflection_map = intensity * 10000; 231 | } 232 | 233 | void standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr& msg) { 234 | mtx_buffer.lock(); 235 | // cout<<"got feature"<header.stamp.toSec() < last_timestamp_lidar) { 237 | ROS_ERROR("lidar loop back, clear buffer"); 238 | lidar_buffer.clear(); 239 | } 240 | // ROS_INFO("get point cloud at time: %.6f", msg->header.stamp.toSec()); 241 | PointCloudXYZI::Ptr ptr(new PointCloudXYZI()); 242 | p_pre->process(msg, ptr); 243 | lidar_buffer.push_back(ptr); 244 | time_buffer.push_back(msg->header.stamp.toSec()); 245 | last_timestamp_lidar = msg->header.stamp.toSec(); 246 | 247 | mtx_buffer.unlock(); 248 | sig_buffer.notify_all(); 249 | } 250 | 251 | void livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr& msg) { 252 | mtx_buffer.lock(); 253 | // cout << "got feature" << endl; 254 | if (msg->header.stamp.toSec() < last_timestamp_lidar) { 255 | ROS_ERROR("lidar loop back, clear buffer"); 256 | lidar_buffer.clear(); 257 | } 258 | // ROS_INFO("get point cloud at time: %.6f", msg->header.stamp.toSec()); 259 | PointCloudXYZI::Ptr ptr(new PointCloudXYZI()); 260 | p_pre->process(msg, ptr); 261 | lidar_buffer.push_back(ptr); 262 | time_buffer.push_back(msg->header.stamp.toSec()); 263 | last_timestamp_lidar = msg->header.stamp.toSec(); 264 | 265 | mtx_buffer.unlock(); 266 | sig_buffer.notify_all(); 267 | } 268 | 269 | void imu_cbk(const sensor_msgs::Imu::ConstPtr& msg_in) { 270 | publish_count++; 271 | sensor_msgs::Imu::Ptr msg(new sensor_msgs::Imu(*msg_in)); 272 | 273 | double timestamp = msg->header.stamp.toSec(); 274 | 275 | mtx_buffer.lock(); 276 | 277 | if (timestamp < last_timestamp_imu) { 278 | ROS_ERROR("imu loop back, clear buffer"); 279 | imu_buffer.clear(); 280 | flg_reset = true; 281 | } 282 | 283 | last_timestamp_imu = timestamp; 284 | 285 | imu_buffer.push_back(msg); 286 | // cout << "got imu: " << timestamp << " imu size " << imu_buffer.size() << 287 | // endl; 288 | mtx_buffer.unlock(); 289 | sig_buffer.notify_all(); 290 | } 291 | 292 | bool sync_packages(MeasureGroup& meas) { 293 | if (!imu_en) { 294 | if (!lidar_buffer.empty()) { 295 | // cout<<"meas.lidar->points.size(): "<points.size()<points.size() <= 1) { 314 | lidar_buffer.pop_front(); 315 | return false; 316 | } 317 | meas.lidar_beg_time = time_buffer.front(); 318 | lidar_end_time = meas.lidar_beg_time + meas.lidar->points.back().curvature / double(1000); 319 | lidar_pushed = true; 320 | } 321 | 322 | if (last_timestamp_imu < lidar_end_time) { 323 | return false; 324 | } 325 | 326 | /*** push imu data, and pop from imu buffer ***/ 327 | double imu_time = imu_buffer.front()->header.stamp.toSec(); 328 | meas.imu.clear(); 329 | while ((!imu_buffer.empty()) && (imu_time < lidar_end_time)) { 330 | imu_time = imu_buffer.front()->header.stamp.toSec(); 331 | if (imu_time > lidar_end_time + 0.02) 332 | break; 333 | meas.imu.push_back(imu_buffer.front()); 334 | imu_buffer.pop_front(); 335 | } 336 | 337 | lidar_buffer.pop_front(); 338 | time_buffer.pop_front(); 339 | lidar_pushed = false; 340 | return true; 341 | } 342 | 343 | void publish_frame_world(const ros::Publisher& pubLaserCloudFullRes, const int point_skip) { 344 | PointCloudXYZI::Ptr laserCloudFullRes(dense_map_en ? feats_undistort : feats_down_body); 345 | int size = laserCloudFullRes->points.size(); 346 | PointCloudXYZI::Ptr laserCloudWorld(new PointCloudXYZI(size, 1)); 347 | for (int i = 0; i < size; i++) { 348 | RGBpointBodyToWorld(&laserCloudFullRes->points[i], &laserCloudWorld->points[i]); 349 | } 350 | PointCloudXYZI::Ptr laserCloudWorldPub(new PointCloudXYZI); 351 | for (int i = 0; i < size; i += point_skip) { 352 | laserCloudWorldPub->points.push_back(laserCloudWorld->points[i]); 353 | } 354 | sensor_msgs::PointCloud2 laserCloudmsg; 355 | 356 | pcl::toROSMsg(*laserCloudWorldPub, laserCloudmsg); 357 | laserCloudmsg.header.stamp = ros::Time::now(); //.fromSec(last_timestamp_lidar); 358 | laserCloudmsg.header.frame_id = "camera_init"; 359 | pubLaserCloudFullRes.publish(laserCloudmsg); 360 | } 361 | 362 | void publish_effect_world(const ros::Publisher& pubLaserCloudEffect, 363 | const ros::Publisher& pubPointWithCov, 364 | const std::vector& ptpl_list) { 365 | pcl::PointCloud::Ptr effect_cloud_world(new pcl::PointCloud); 366 | PointCloudXYZI::Ptr laserCloudWorld(new PointCloudXYZI(effct_feat_num, 1)); 367 | visualization_msgs::MarkerArray ma_line; 368 | visualization_msgs::Marker m_line; 369 | m_line.type = visualization_msgs::Marker::LINE_LIST; 370 | m_line.action = visualization_msgs::Marker::ADD; 371 | m_line.ns = "lines"; 372 | m_line.color.a = 0.5; // Don't forget to set the alpha! 373 | m_line.color.r = 1.0; 374 | m_line.color.g = 1.0; 375 | m_line.color.b = 1.0; 376 | m_line.scale.x = 0.01; 377 | m_line.pose.orientation.w = 1.0; 378 | m_line.header.frame_id = "camera_init"; 379 | for (int i = 0; i < ptpl_list.size(); i++) { 380 | Eigen::Vector3d p_c = ptpl_list[i].point; 381 | Eigen::Vector3d p_w = state.rot_end * (p_c) + state.pos_end; 382 | pcl::PointXYZRGB pi; 383 | pi.x = p_w[0]; 384 | pi.y = p_w[1]; 385 | pi.z = p_w[2]; 386 | // float v = laserCloudWorld->points[i].intensity / 200; 387 | // v = 1.0 - v; 388 | // uint8_t r, g, b; 389 | // mapJet(v, 0, 1, r, g, b); 390 | // pi.r = r; 391 | // pi.g = g; 392 | // pi.b = b; 393 | effect_cloud_world->points.push_back(pi); 394 | m_line.points.clear(); 395 | geometry_msgs::Point p; 396 | p.x = p_w[0]; 397 | p.y = p_w[1]; 398 | p.z = p_w[2]; 399 | m_line.points.push_back(p); 400 | p.x = ptpl_list[i].center(0); 401 | p.y = ptpl_list[i].center(1); 402 | p.z = ptpl_list[i].center(2); 403 | m_line.points.push_back(p); 404 | ma_line.markers.push_back(m_line); 405 | m_line.id++; 406 | } 407 | int max_num = 20000; 408 | for (int i = ptpl_list.size(); i < max_num; i++) { 409 | m_line.color.a = 0; 410 | ma_line.markers.push_back(m_line); 411 | m_line.id++; 412 | } 413 | pubPointWithCov.publish(ma_line); 414 | 415 | sensor_msgs::PointCloud2 laserCloudFullRes3; 416 | pcl::toROSMsg(*effect_cloud_world, laserCloudFullRes3); 417 | laserCloudFullRes3.header.stamp = ros::Time::now(); //.fromSec(last_timestamp_lidar); 418 | laserCloudFullRes3.header.frame_id = "camera_init"; 419 | pubLaserCloudEffect.publish(laserCloudFullRes3); 420 | } 421 | 422 | void publish_no_effect(const ros::Publisher& pubLaserCloudNoEffect) { 423 | sensor_msgs::PointCloud2 laserCloudFullRes3; 424 | pcl::toROSMsg(*laserCloudNoeffect, laserCloudFullRes3); 425 | laserCloudFullRes3.header.stamp = ros::Time::now(); //.fromSec(last_timestamp_lidar); 426 | laserCloudFullRes3.header.frame_id = "camera_init"; 427 | pubLaserCloudNoEffect.publish(laserCloudFullRes3); 428 | } 429 | 430 | void publish_effect(const ros::Publisher& pubLaserCloudEffect) { 431 | pcl::PointCloud::Ptr effect_cloud_world(new pcl::PointCloud); 432 | PointCloudXYZI::Ptr laserCloudWorld(new PointCloudXYZI(effct_feat_num, 1)); 433 | for (int i = 0; i < effct_feat_num; i++) { 434 | RGBpointBodyToWorld(&laserCloudOri->points[i], &laserCloudWorld->points[i]); 435 | pcl::PointXYZRGB pi; 436 | pi.x = laserCloudWorld->points[i].x; 437 | pi.y = laserCloudWorld->points[i].y; 438 | pi.z = laserCloudWorld->points[i].z; 439 | float v = laserCloudWorld->points[i].intensity / 100; 440 | v = 1.0 - v; 441 | uint8_t r, g, b; 442 | mapJet(v, 0, 1, r, g, b); 443 | pi.r = r; 444 | pi.g = g; 445 | pi.b = b; 446 | effect_cloud_world->points.push_back(pi); 447 | } 448 | 449 | sensor_msgs::PointCloud2 laserCloudFullRes3; 450 | pcl::toROSMsg(*laserCloudWorld, laserCloudFullRes3); 451 | laserCloudFullRes3.header.stamp = ros::Time::now(); //.fromSec(last_timestamp_lidar); 452 | laserCloudFullRes3.header.frame_id = "camera_init"; 453 | pubLaserCloudEffect.publish(laserCloudFullRes3); 454 | } 455 | 456 | template 457 | void set_posestamp(T& out) { 458 | out.position.x = state.pos_end(0); 459 | out.position.y = state.pos_end(1); 460 | out.position.z = state.pos_end(2); 461 | out.orientation.x = geoQuat.x; 462 | out.orientation.y = geoQuat.y; 463 | out.orientation.z = geoQuat.z; 464 | out.orientation.w = geoQuat.w; 465 | } 466 | 467 | void publish_odometry(const ros::Publisher& pubOdomAftMapped) { 468 | odomAftMapped.header.frame_id = "camera_init"; 469 | odomAftMapped.child_frame_id = "aft_mapped"; 470 | odomAftMapped.header.stamp = ros::Time::now(); // ros::Time().fromSec(last_timestamp_lidar); 471 | set_posestamp(odomAftMapped.pose.pose); 472 | static tf::TransformBroadcaster br; 473 | tf::Transform transform; 474 | tf::Quaternion q; 475 | transform.setOrigin(tf::Vector3(state.pos_end(0), state.pos_end(1), state.pos_end(2))); 476 | q.setW(geoQuat.w); 477 | q.setX(geoQuat.x); 478 | q.setY(geoQuat.y); 479 | q.setZ(geoQuat.z); 480 | transform.setRotation(q); 481 | br.sendTransform( 482 | tf::StampedTransform(transform, odomAftMapped.header.stamp, "camera_init", "aft_mapped")); 483 | pubOdomAftMapped.publish(odomAftMapped); 484 | } 485 | 486 | void publish_mavros(const ros::Publisher& mavros_pose_publisher) { 487 | msg_body_pose.header.stamp = ros::Time::now(); 488 | msg_body_pose.header.frame_id = "camera_odom_frame"; 489 | set_posestamp(msg_body_pose.pose); 490 | mavros_pose_publisher.publish(msg_body_pose); 491 | } 492 | 493 | void publish_path(const ros::Publisher pubPath) { 494 | set_posestamp(msg_body_pose.pose); 495 | msg_body_pose.header.stamp = ros::Time::now(); 496 | msg_body_pose.header.frame_id = "camera_init"; 497 | path.poses.push_back(msg_body_pose); 498 | pubPath.publish(path); 499 | } 500 | 501 | void get_leaf_nums(size_t& voxel_map_size, OctoTree* voxel) { 502 | if (voxel->layer_ == voxel->max_layer_) { 503 | return; 504 | } 505 | for (size_t leafnum = 0; leafnum < 8; leafnum++) { 506 | if (voxel->leaves_[leafnum] != nullptr) { 507 | voxel_map_size++; 508 | get_leaf_nums(voxel_map_size, voxel->leaves_[leafnum]); 509 | } 510 | } 511 | } 512 | 513 | size_t get_voxel_map_size(std::unordered_map& voxel_map) { 514 | size_t voxel_map_size = 0; 515 | for (auto iter = voxel_map.begin(); iter != voxel_map.end(); ++iter) { 516 | voxel_map_size++; 517 | get_leaf_nums(voxel_map_size, iter->second); 518 | } 519 | return voxel_map_size; 520 | } 521 | 522 | void get_leaf_point_size(size_t& voxel_map_point_size, OctoTree* voxel) { 523 | voxel_map_point_size += (voxel->temp_points_.size() + voxel->new_points_.size()); 524 | for (size_t leafnum = 0; leafnum < 8; leafnum++) { 525 | if (voxel->leaves_[leafnum] != nullptr) { 526 | get_leaf_point_size(voxel_map_point_size, voxel->leaves_[leafnum]); 527 | } 528 | } 529 | } 530 | 531 | size_t get_voxel_map_point_size(std::unordered_map& voxel_map) { 532 | size_t voxel_map_point_size = 0; 533 | for (auto iter = voxel_map.begin(); iter != voxel_map.end(); ++iter) { 534 | get_leaf_point_size(voxel_map_point_size, iter->second); 535 | } 536 | return voxel_map_point_size; 537 | } 538 | 539 | int main(int argc, char** argv) { 540 | ros::init(argc, argv, "voxelMapping"); 541 | ros::NodeHandle nh; 542 | 543 | double ranging_cov = 0.0; 544 | double angle_cov = 0.0; 545 | std::vector layer_point_size; 546 | 547 | // Enable voxel merging 548 | bool enable_voxel_merging = false; 549 | bool pub_merged_voxel = false; 550 | double merge_theta_thresh = 0.05; 551 | double merge_dist_thresh = 0.05; 552 | double merge_cov_min_eigen_val_thresh = 0.002; 553 | double merge_x_coord_diff_thresh = 5.0; 554 | double merge_y_coord_diff_thresh = 5.0; 555 | 556 | // cummon params 557 | nh.param("common/lid_topic", lid_topic, "/livox/lidar"); 558 | nh.param("common/imu_topic", imu_topic, "/livox/imu"); 559 | 560 | // noise model params 561 | nh.param("noise_model/ranging_cov", ranging_cov, 0.02); 562 | nh.param("noise_model/angle_cov", angle_cov, 0.05); 563 | nh.param("noise_model/gyr_cov_scale", gyr_cov_scale, 0.1); 564 | nh.param("noise_model/acc_cov_scale", acc_cov_scale, 0.1); 565 | 566 | // imu params, current version does not support imu 567 | nh.param("imu/imu_en", imu_en, false); 568 | nh.param>("imu/extrinsic_T", extrinT, vector()); 569 | nh.param>("imu/extrinsic_R", extrinR, vector()); 570 | 571 | // mapping algorithm params 572 | nh.param("mapping/max_iteration", NUM_MAX_ITERATIONS, 4); 573 | nh.param>("mapping/layer_point_size", layer_point_size, vector()); 574 | nh.param("mapping/max_layer", max_layer, 2); 575 | nh.param("mapping/voxel_size", max_voxel_size, 1.0); 576 | nh.param("mapping/down_sample_size", filter_size_surf_min, 0.5); 577 | // std::cout << "filter_size_surf_min:" << filter_size_surf_min << std::endl; 578 | nh.param("mapping/plannar_threshold", min_eigen_value, 0.01); 579 | 580 | // vocel merging params 581 | nh.param("voxel_merging/enable_voxel_merging", enable_voxel_merging, false); 582 | nh.param("voxel_merging/merge_theta_thresh", merge_theta_thresh, 0.05); 583 | nh.param("voxel_merging/merge_dist_thresh", merge_dist_thresh, 0.05); 584 | nh.param("voxel_merging/merge_cov_min_eigen_val_thresh", 585 | merge_cov_min_eigen_val_thresh, 586 | 0.001); 587 | nh.param("voxel_merging/merge_x_coord_diff_thresh", merge_x_coord_diff_thresh, 5.0); 588 | nh.param("voxel_merging/merge_y_coord_diff_thresh", merge_y_coord_diff_thresh, 5.0); 589 | 590 | // preprocess params 591 | nh.param("preprocess/blind", p_pre->blind, 0.01); 592 | nh.param("preprocess/calib_laser", calib_laser, false); 593 | nh.param("preprocess/lidar_type", p_pre->lidar_type, AVIA); 594 | nh.param("preprocess/scan_line", p_pre->N_SCANS, 16); 595 | nh.param("preprocess/point_filter_num", p_pre->point_filter_num, 2); 596 | 597 | // visualization params 598 | nh.param("visualization/pub_voxel_map", publish_voxel_map, false); 599 | nh.param("visualization/publish_max_voxel_layer", publish_max_voxel_layer, 0); 600 | nh.param("visualization/pub_point_cloud", publish_point_cloud, true); 601 | nh.param("visualization/pub_point_cloud_skip", pub_point_cloud_skip, 1); 602 | nh.param("visualization/dense_map_enable", dense_map_en, false); 603 | nh.param("visualization/pub_merged_voxel", pub_merged_voxel, false); 604 | 605 | // result params 606 | nh.param("Result/write_kitti_log", write_kitti_log, false); 607 | nh.param("Result/result_path", result_path, ""); 608 | 609 | cout << "p_pre->lidar_type " << p_pre->lidar_type << endl; 610 | for (int i = 0; i < layer_point_size.size(); i++) { 611 | layer_size.push_back(layer_point_size[i]); 612 | } 613 | 614 | ros::Subscriber sub_pcl = p_pre->lidar_type == AVIA ? 615 | nh.subscribe(lid_topic, 200000, livox_pcl_cbk) : 616 | nh.subscribe(lid_topic, 200000, standard_pcl_cbk); 617 | ros::Subscriber sub_imu; 618 | if (imu_en) { 619 | sub_imu = nh.subscribe(imu_topic, 200000, imu_cbk); 620 | } 621 | 622 | ros::Publisher pubLaserCloudFullRes = nh.advertise("/cloud_registered", 623 | 100); 624 | ros::Publisher pubLaserCloudEffect = nh.advertise("/cloud_effected", 625 | 100); 626 | ros::Publisher pubOdomAftMapped = nh.advertise("/aft_mapped_to_init", 10); 627 | ros::Publisher pubPath = nh.advertise("/path", 10); 628 | ros::Publisher voxel_map_pub = nh.advertise("/planes", 10000); 629 | ros::Publisher merged_voxel_publisher = nh.advertise( 630 | "/merged_voxels", 631 | 10000); 632 | 633 | path.header.stamp = ros::Time::now(); 634 | path.header.frame_id = "camera_init"; 635 | 636 | /*** variables definition ***/ 637 | VD(DIM_STATE) solution; 638 | MD(DIM_STATE, DIM_STATE) G, H_T_H, I_STATE; 639 | V3D rot_add, t_add; 640 | StatesGroup state_propagat; 641 | PointType pointOri, pointSel, coeff; 642 | PointCloudXYZI::Ptr corr_normvect(new PointCloudXYZI(100000, 1)); 643 | uint32_t frame_num = 0; 644 | double deltaT, deltaR, aver_time_consu = 0; 645 | bool flg_EKF_inited, flg_EKF_converged, EKF_stop_flg = 0, is_first_frame = true; 646 | downSizeFilterSurf.setLeafSize(filter_size_surf_min, filter_size_surf_min, filter_size_surf_min); 647 | 648 | shared_ptr p_imu(new ImuProcess()); 649 | p_imu->imu_en = imu_en; 650 | Eigen::Vector3d extT; 651 | Eigen::Matrix3d extR; 652 | extT << extrinT[0], extrinT[1], extrinT[2]; 653 | extR << extrinR[0], extrinR[1], extrinR[2], extrinR[3], extrinR[4], extrinR[5], extrinR[6], 654 | extrinR[7], extrinR[8]; 655 | p_imu->set_extrinsic(extT, extR); 656 | 657 | // Current version do not support imu. 658 | // if (imu_en) { 659 | // std::cout << "use imu" << std::endl; 660 | // } else { 661 | // std::cout << "no imu" << std::endl; 662 | // } 663 | 664 | p_imu->set_gyr_cov_scale(V3D(gyr_cov_scale, gyr_cov_scale, gyr_cov_scale)); 665 | p_imu->set_acc_cov_scale(V3D(acc_cov_scale, acc_cov_scale, acc_cov_scale)); 666 | p_imu->set_gyr_bias_cov(V3D(0.00001, 0.00001, 0.00001)); 667 | p_imu->set_acc_bias_cov(V3D(0.00001, 0.00001, 0.00001)); 668 | 669 | G.setZero(); 670 | H_T_H.setZero(); 671 | I_STATE.setIdentity(); 672 | 673 | /*** debug record ***/ 674 | FILE* fp_kitti; 675 | 676 | if (write_kitti_log) { 677 | fp_kitti = fopen(result_path.c_str(), "w"); 678 | } 679 | 680 | signal(SIGINT, SigHandle); 681 | ros::Rate rate(5000); 682 | bool status = ros::ok(); 683 | 684 | // for Plane Map 685 | bool init_map = false; 686 | std::unordered_map voxel_map; 687 | last_rot << 1, 0, 0, 0, 1, 0, 0, 0, 1; 688 | 689 | // Plane hash map 690 | std::unordered_map plane_hash_map; 691 | std::unordered_map> merge_node_table; 692 | 693 | size_t idx = 0; 694 | while (status) { 695 | if (flg_exit) 696 | break; 697 | ros::spinOnce(); 698 | if (sync_packages(Measures)) { 699 | idx++; 700 | // std::cout << "sync once" << std::endl; 701 | if (flg_reset) { 702 | ROS_WARN("reset when rosbag play back"); 703 | p_imu->Reset(); 704 | flg_reset = false; 705 | continue; 706 | } 707 | std::cout << "scanIdx:" << scanIdx << std::endl; 708 | double t0, t1, t2, t3, t4, t5, match_start, match_time, solve_start, svd_time; 709 | match_time = 0; 710 | solve_time = 0; 711 | svd_time = 0; 712 | 713 | // std::cout << " init rot cov:" << std::endl 714 | // << state.cov.block<3, 3>(0, 0) << std::endl; 715 | auto undistort_start = std::chrono::high_resolution_clock::now(); 716 | p_imu->Process(Measures, state, feats_undistort); 717 | auto undistort_end = std::chrono::high_resolution_clock::now(); 718 | auto undistort_time = std::chrono::duration_cast>( 719 | undistort_end - undistort_start) 720 | .count() * 721 | 1000; 722 | if (calib_laser) { 723 | // calib the vertical angle for kitti dataset 724 | for (size_t i = 0; i < feats_undistort->size(); i++) { 725 | PointType pi = feats_undistort->points[i]; 726 | double range = sqrt(pi.x * pi.x + pi.y * pi.y + pi.z * pi.z); 727 | double calib_vertical_angle = deg2rad(0.15); 728 | double vertical_angle = asin(pi.z / range) + calib_vertical_angle; 729 | double horizon_angle = atan2(pi.y, pi.x); 730 | pi.z = range * sin(vertical_angle); 731 | double project_len = range * cos(vertical_angle); 732 | pi.x = project_len * cos(horizon_angle); 733 | pi.y = project_len * sin(horizon_angle); 734 | feats_undistort->points[i] = pi; 735 | } 736 | } 737 | state_propagat = state; 738 | 739 | if (is_first_frame) { 740 | first_lidar_time = Measures.lidar_beg_time; 741 | is_first_frame = false; 742 | } 743 | 744 | if (feats_undistort->empty() || (feats_undistort == NULL)) { 745 | p_imu->first_lidar_time = first_lidar_time; 746 | cout << "FAST-LIO not ready" << endl; 747 | continue; 748 | } 749 | 750 | flg_EKF_inited = (Measures.lidar_beg_time - first_lidar_time) < INIT_TIME ? false : true; 751 | if (flg_EKF_inited && !init_map) { 752 | pcl::PointCloud::Ptr world_lidar(new pcl::PointCloud); 753 | Eigen::Quaterniond q(state.rot_end); 754 | transformLidar(state, p_imu, feats_undistort, world_lidar); 755 | std::vector pv_list; 756 | for (size_t i = 0; i < world_lidar->size(); i++) { 757 | pointWithCov pv; 758 | pv.point << world_lidar->points[i].x, world_lidar->points[i].y, world_lidar->points[i].z; 759 | V3D point_this(feats_undistort->points[i].x, 760 | feats_undistort->points[i].y, 761 | feats_undistort->points[i].z); 762 | // if z=0, error will occur in calcBodyCov. To be solved 763 | if (point_this[2] == 0) { 764 | point_this[2] = 0.001; 765 | } 766 | M3D cov; 767 | calcBodyCov(point_this, ranging_cov, angle_cov, cov); 768 | 769 | point_this += Lidar_offset_to_IMU; 770 | M3D point_crossmat; 771 | point_crossmat << SKEW_SYM_MATRX(point_this); 772 | cov = state.rot_end * cov * state.rot_end.transpose() + 773 | (-point_crossmat) * state.cov.block<3, 3>(0, 0) * (-point_crossmat).transpose() + 774 | state.cov.block<3, 3>(3, 3); 775 | pv.cov = cov; 776 | pv_list.push_back(pv); 777 | Eigen::Vector3d sigma_pv = pv.cov.diagonal(); 778 | sigma_pv[0] = sqrt(sigma_pv[0]); 779 | sigma_pv[1] = sqrt(sigma_pv[1]); 780 | sigma_pv[2] = sqrt(sigma_pv[2]); 781 | } 782 | 783 | buildVoxelMap(enable_voxel_merging, 784 | pv_list, 785 | max_voxel_size, 786 | max_layer, 787 | layer_size, 788 | min_eigen_value, 789 | voxel_map, 790 | plane_hash_map, 791 | merge_node_table, 792 | merge_theta_thresh, 793 | merge_dist_thresh, 794 | merge_cov_min_eigen_val_thresh, 795 | merge_x_coord_diff_thresh, 796 | merge_y_coord_diff_thresh); 797 | 798 | std::cout << "build voxel map" << std::endl; 799 | if (write_kitti_log) { 800 | kitti_log(fp_kitti); 801 | } 802 | 803 | scanIdx++; 804 | if (publish_voxel_map) { 805 | pubVoxelMap(voxel_map, publish_max_voxel_layer, voxel_map_pub); 806 | } 807 | init_map = true; 808 | continue; 809 | } 810 | 811 | /*** downsample the feature points in a scan ***/ 812 | auto t_downsample_start = std::chrono::high_resolution_clock::now(); 813 | downSizeFilterSurf.setInputCloud(feats_undistort); 814 | downSizeFilterSurf.filter(*feats_down_body); 815 | sort(feats_down_body->points.begin(), feats_down_body->points.end(), time_list); 816 | 817 | auto t_downsample_end = std::chrono::high_resolution_clock::now(); 818 | std::cout << " feats size:" << feats_undistort->size() 819 | << ", down size:" << feats_down_body->size() << std::endl; 820 | auto t_downsample = std::chrono::duration_cast>( 821 | t_downsample_end - t_downsample_start) 822 | .count() * 823 | 1000; 824 | 825 | int rematch_num = 0; 826 | bool nearest_search_en = true; 827 | double total_residual; 828 | 829 | scan_match_time = 0.0; 830 | 831 | std::vector body_var; 832 | std::vector crossmat_list; 833 | 834 | /*** iterated state estimation ***/ 835 | auto calc_point_cov_start = std::chrono::high_resolution_clock::now(); 836 | for (size_t i = 0; i < feats_down_body->size(); i++) { 837 | V3D point_this(feats_down_body->points[i].x, 838 | feats_down_body->points[i].y, 839 | feats_down_body->points[i].z); 840 | if (point_this[2] == 0) { 841 | point_this[2] = 0.001; 842 | } 843 | M3D cov; 844 | calcBodyCov(point_this, ranging_cov, angle_cov, cov); 845 | M3D point_crossmat; 846 | point_crossmat << SKEW_SYM_MATRX(point_this); 847 | crossmat_list.push_back(point_crossmat); 848 | M3D rot_var = state.cov.block<3, 3>(0, 0); 849 | M3D t_var = state.cov.block<3, 3>(3, 3); 850 | body_var.push_back(cov); 851 | } 852 | auto calc_point_cov_end = std::chrono::high_resolution_clock::now(); 853 | double calc_point_cov_time = std::chrono::duration_cast>( 854 | calc_point_cov_end - calc_point_cov_start) 855 | .count() * 856 | 1000; 857 | for (iterCount = 0; iterCount < NUM_MAX_ITERATIONS; iterCount++) { 858 | laserCloudOri->clear(); 859 | laserCloudNoeffect->clear(); 860 | corr_normvect->clear(); 861 | total_residual = 0.0; 862 | 863 | std::vector r_list; 864 | std::vector ptpl_list; 865 | /** LiDAR match based on 3 sigma criterion **/ 866 | 867 | vector pv_list; 868 | std::vector var_list; 869 | pcl::PointCloud::Ptr world_lidar(new pcl::PointCloud); 870 | transformLidar(state, p_imu, feats_down_body, world_lidar); 871 | for (size_t i = 0; i < feats_down_body->size(); i++) { 872 | pointWithCov pv; 873 | pv.point << feats_down_body->points[i].x, feats_down_body->points[i].y, 874 | feats_down_body->points[i].z; 875 | pv.point_world << world_lidar->points[i].x, world_lidar->points[i].y, 876 | world_lidar->points[i].z; 877 | M3D cov = body_var[i]; 878 | M3D point_crossmat = crossmat_list[i]; 879 | M3D rot_var = state.cov.block<3, 3>(0, 0); 880 | M3D t_var = state.cov.block<3, 3>(3, 3); 881 | cov = state.rot_end * cov * state.rot_end.transpose() + 882 | (-point_crossmat) * rot_var * (-point_crossmat.transpose()) + t_var; 883 | pv.cov = cov; 884 | pv_list.push_back(pv); 885 | var_list.push_back(cov); 886 | } 887 | auto scan_match_time_start = std::chrono::high_resolution_clock::now(); 888 | auto scan_match_time_end = std::chrono::high_resolution_clock::now(); 889 | std::vector non_match_list; 890 | { 891 | BuildResidualListOMP(voxel_map, 892 | max_voxel_size, 893 | 3.0, 894 | max_layer, 895 | pv_list, 896 | ptpl_list, 897 | non_match_list); 898 | 899 | scan_match_time_end = std::chrono::high_resolution_clock::now(); 900 | 901 | effct_feat_num = 0; 902 | for (int i = 0; i < ptpl_list.size(); i++) { 903 | PointType pi_body; 904 | PointType pi_world; 905 | PointType pl; 906 | pi_body.x = ptpl_list[i].point(0); 907 | pi_body.y = ptpl_list[i].point(1); 908 | pi_body.z = ptpl_list[i].point(2); 909 | pointBodyToWorld(&pi_body, &pi_world); 910 | pl.x = ptpl_list[i].normal(0); 911 | pl.y = ptpl_list[i].normal(1); 912 | pl.z = ptpl_list[i].normal(2); 913 | effct_feat_num++; 914 | float dis = (pi_world.x * pl.x + pi_world.y * pl.y + pi_world.z * pl.z + 915 | ptpl_list[i].d); 916 | pl.intensity = dis; 917 | laserCloudOri->push_back(pi_body); 918 | corr_normvect->push_back(pl); 919 | total_residual += fabs(dis); 920 | } 921 | } 922 | res_mean_last = total_residual / effct_feat_num; 923 | scan_match_time += std::chrono::duration_cast>( 924 | scan_match_time_end - scan_match_time_start) 925 | .count() * 926 | 1000; 927 | 928 | // cout << "[ Matching ]: Time:" 929 | // << std::chrono::duration_cast>( 930 | // scan_match_time_end - scan_match_time_start) 931 | // .count() * 932 | // 1000 933 | // << " ms Effective feature num: " << effct_feat_num 934 | // << " All num:" << feats_down_body->size() << " res_mean_last 935 | // " 936 | // << res_mean_last << endl; 937 | 938 | auto t_solve_start = std::chrono::high_resolution_clock::now(); 939 | 940 | /*** Computation of Measuremnt Jacobian matrix H and measurents vector 941 | * ***/ 942 | MatrixXd Hsub(effct_feat_num, 6); 943 | MatrixXd Hsub_T_R_inv(6, effct_feat_num); 944 | VectorXd R_inv(effct_feat_num); 945 | VectorXd meas_vec(effct_feat_num); 946 | 947 | for (int i = 0; i < effct_feat_num; i++) { 948 | const PointType& laser_p = laserCloudOri->points[i]; 949 | V3D point_this(laser_p.x, laser_p.y, laser_p.z); 950 | M3D cov; 951 | if (calib_laser) { 952 | calcBodyCov(point_this, ranging_cov, CALIB_ANGLE_COV, cov); 953 | } else { 954 | calcBodyCov(point_this, ranging_cov, angle_cov, cov); 955 | } 956 | 957 | cov = state.rot_end * cov * state.rot_end.transpose(); 958 | M3D point_crossmat; 959 | point_crossmat << SKEW_SYM_MATRX(point_this); 960 | const PointType& norm_p = corr_normvect->points[i]; 961 | V3D norm_vec(norm_p.x, norm_p.y, norm_p.z); 962 | V3D point_world = state.rot_end * point_this + state.pos_end; 963 | // /*** get the normal vector of closest surface/corner ***/ 964 | Eigen::Matrix J_nq; 965 | J_nq.block<1, 3>(0, 0) = point_world - ptpl_list[i].center; 966 | J_nq.block<1, 3>(0, 3) = -ptpl_list[i].normal; 967 | double sigma_l = J_nq * ptpl_list[i].plane_cov * J_nq.transpose(); 968 | R_inv(i) = 1.0 / (sigma_l + norm_vec.transpose() * cov * norm_vec); 969 | double ranging_dis = point_this.norm(); 970 | laserCloudOri->points[i].intensity = sqrt(R_inv(i)); 971 | laserCloudOri->points[i].normal_x = corr_normvect->points[i].intensity; 972 | laserCloudOri->points[i].normal_y = sqrt(sigma_l); 973 | laserCloudOri->points[i].normal_z = sqrt(norm_vec.transpose() * cov * norm_vec); 974 | laserCloudOri->points[i].curvature = sqrt(sigma_l + 975 | norm_vec.transpose() * cov * norm_vec); 976 | 977 | /*** calculate the Measuremnt Jacobian matrix H ***/ 978 | V3D A(point_crossmat * state.rot_end.transpose() * norm_vec); 979 | Hsub.row(i) << VEC_FROM_ARRAY(A), norm_p.x, norm_p.y, norm_p.z; 980 | Hsub_T_R_inv.col(i) << A[0] * R_inv(i), A[1] * R_inv(i), A[2] * R_inv(i), 981 | norm_p.x * R_inv(i), norm_p.y * R_inv(i), norm_p.z * R_inv(i); 982 | /*** Measuremnt: distance to the closest surface/corner ***/ 983 | meas_vec(i) = -norm_p.intensity; 984 | } 985 | MatrixXd K(DIM_STATE, effct_feat_num); 986 | 987 | EKF_stop_flg = false; 988 | flg_EKF_converged = false; 989 | 990 | /*** Iterative Kalman Filter Update ***/ 991 | if (!flg_EKF_inited) { 992 | cout << "||||||||||Initiallizing LiDar||||||||||" << endl; 993 | /*** only run in initialization period ***/ 994 | MatrixXd H_init(MD(9, DIM_STATE)::Zero()); 995 | MatrixXd z_init(VD(9)::Zero()); 996 | H_init.block<3, 3>(0, 0) = M3D::Identity(); 997 | H_init.block<3, 3>(3, 3) = M3D::Identity(); 998 | H_init.block<3, 3>(6, 15) = M3D::Identity(); 999 | z_init.block<3, 1>(0, 0) = -Log(state.rot_end); 1000 | z_init.block<3, 1>(0, 0) = -state.pos_end; 1001 | 1002 | auto H_init_T = H_init.transpose(); 1003 | auto&& K_init = state.cov * H_init_T * 1004 | (H_init * state.cov * H_init_T + 0.0001 * MD(9, 9)::Identity()).inverse(); 1005 | solution = K_init * z_init; 1006 | 1007 | state.resetpose(); 1008 | EKF_stop_flg = true; 1009 | } else { 1010 | auto&& Hsub_T = Hsub.transpose(); 1011 | H_T_H.block<6, 6>(0, 0) = Hsub_T_R_inv * Hsub; 1012 | MD(DIM_STATE, DIM_STATE)&& K_1 = (H_T_H + (state.cov).inverse()).inverse(); 1013 | K = K_1.block(0, 0) * Hsub_T_R_inv; 1014 | auto vec = state_propagat - state; 1015 | solution = K * meas_vec + vec - K * Hsub * vec.block<6, 1>(0, 0); 1016 | 1017 | int minRow, minCol; 1018 | if (0) // if(V.minCoeff(&minRow, &minCol) < 1.0f) 1019 | { 1020 | VD(6) V = H_T_H.block<6, 6>(0, 0).eigenvalues().real(); 1021 | cout << "!!!!!! Degeneration Happend, eigen values: " << V.transpose() << endl; 1022 | EKF_stop_flg = true; 1023 | solution.block<6, 1>(9, 0).setZero(); 1024 | } 1025 | 1026 | state += solution; 1027 | 1028 | rot_add = solution.block<3, 1>(0, 0); 1029 | t_add = solution.block<3, 1>(3, 0); 1030 | 1031 | if ((rot_add.norm() * 57.3 < 0.01) && (t_add.norm() * 100 < 0.015)) { 1032 | flg_EKF_converged = true; 1033 | } 1034 | 1035 | deltaR = rot_add.norm() * 57.3; 1036 | deltaT = t_add.norm() * 100; 1037 | } 1038 | euler_cur = RotMtoEuler(state.rot_end); 1039 | /*** Rematch Judgement ***/ 1040 | nearest_search_en = false; 1041 | if (flg_EKF_converged || ((rematch_num == 0) && (iterCount == (NUM_MAX_ITERATIONS - 2)))) { 1042 | nearest_search_en = true; 1043 | rematch_num++; 1044 | } 1045 | 1046 | /*** Convergence Judgements and Covariance Update ***/ 1047 | if (!EKF_stop_flg && (rematch_num >= 2 || (iterCount == NUM_MAX_ITERATIONS - 1))) { 1048 | if (flg_EKF_inited) { 1049 | /*** Covariance Update ***/ 1050 | G.setZero(); 1051 | G.block(0, 0) = K * Hsub; 1052 | state.cov = (I_STATE - G) * state.cov; 1053 | total_distance += (state.pos_end - position_last).norm(); 1054 | position_last = state.pos_end; 1055 | 1056 | geoQuat = tf::createQuaternionMsgFromRollPitchYaw(euler_cur(0), 1057 | euler_cur(1), 1058 | euler_cur(2)); 1059 | 1060 | VD(DIM_STATE) K_sum = K.rowwise().sum(); 1061 | VD(DIM_STATE) P_diag = state.cov.diagonal(); 1062 | } 1063 | EKF_stop_flg = true; 1064 | } 1065 | auto t_solve_end = std::chrono::high_resolution_clock::now(); 1066 | solve_time += std::chrono::duration_cast>(t_solve_end - 1067 | t_solve_start) 1068 | .count() * 1069 | 1000; 1070 | 1071 | if (EKF_stop_flg) 1072 | break; 1073 | } 1074 | /*** add the points to the voxel map ***/ 1075 | auto map_incremental_start = std::chrono::high_resolution_clock::now(); 1076 | pcl::PointCloud::Ptr world_lidar(new pcl::PointCloud); 1077 | transformLidar(state, p_imu, feats_down_body, world_lidar); 1078 | std::vector pv_list; 1079 | for (size_t i = 0; i < world_lidar->size(); i++) { 1080 | pointWithCov pv; 1081 | pv.point << world_lidar->points[i].x, world_lidar->points[i].y, world_lidar->points[i].z; 1082 | M3D point_crossmat = crossmat_list[i]; 1083 | M3D cov = body_var[i]; 1084 | cov = state.rot_end * cov * state.rot_end.transpose() + 1085 | (-point_crossmat) * state.cov.block<3, 3>(0, 0) * (-point_crossmat).transpose() + 1086 | state.cov.block<3, 3>(3, 3); 1087 | pv.cov = cov; 1088 | pv_list.push_back(pv); 1089 | } 1090 | std::sort(pv_list.begin(), pv_list.end(), var_contrast); 1091 | updateVoxelMap(enable_voxel_merging, 1092 | pv_list, 1093 | max_voxel_size, 1094 | max_layer, 1095 | layer_size, 1096 | min_eigen_value, 1097 | voxel_map, 1098 | plane_hash_map, 1099 | merge_node_table, 1100 | frame_num, 1101 | merge_theta_thresh, 1102 | merge_dist_thresh, 1103 | merge_cov_min_eigen_val_thresh, 1104 | merge_x_coord_diff_thresh, 1105 | merge_y_coord_diff_thresh); 1106 | auto map_incremental_end = std::chrono::high_resolution_clock::now(); 1107 | map_incremental_time = std::chrono::duration_cast>( 1108 | map_incremental_end - map_incremental_start) 1109 | .count() * 1110 | 1000; 1111 | 1112 | total_time = t_downsample + scan_match_time + solve_time + map_incremental_time + 1113 | undistort_time + calc_point_cov_time; 1114 | /******* Publish function: *******/ 1115 | { 1116 | publish_odometry(pubOdomAftMapped); 1117 | publish_path(pubPath); 1118 | tf::Transform transform; 1119 | tf::Quaternion q; 1120 | transform.setOrigin(tf::Vector3(state.pos_end(0), state.pos_end(1), state.pos_end(2))); 1121 | q.setW(geoQuat.w); 1122 | q.setX(geoQuat.x); 1123 | q.setY(geoQuat.y); 1124 | q.setZ(geoQuat.z); 1125 | transform.setRotation(q); 1126 | transformLidar(state, p_imu, feats_down_body, world_lidar); 1127 | sensor_msgs::PointCloud2 pub_cloud; 1128 | pcl::toROSMsg(*world_lidar, pub_cloud); 1129 | pub_cloud.header.stamp = ros::Time::now(); //.fromSec(last_timestamp_lidar); 1130 | pub_cloud.header.frame_id = "camera_init"; 1131 | if (publish_point_cloud) { 1132 | publish_frame_world(pubLaserCloudFullRes, pub_point_cloud_skip); 1133 | } 1134 | 1135 | if (publish_voxel_map) { 1136 | pubVoxelMap(voxel_map, publish_max_voxel_layer, voxel_map_pub); 1137 | } 1138 | 1139 | publish_effect(pubLaserCloudEffect); 1140 | 1141 | if (pub_merged_voxel && (frame_num % 30 == 0)) { 1142 | PublishMergedVoxelPlane(voxel_map, 1143 | merge_node_table, 1144 | merged_voxel_publisher, 1145 | max_voxel_size); 1146 | } 1147 | } 1148 | 1149 | frame_num++; 1150 | mean_raw_points = mean_raw_points * (frame_num - 1) / frame_num + 1151 | (double) (feats_undistort->size()) / frame_num; 1152 | mean_ds_points = mean_ds_points * (frame_num - 1) / frame_num + 1153 | (double) (feats_down_body->size()) / frame_num; 1154 | mean_effect_points = mean_effect_points * (frame_num - 1) / frame_num + 1155 | (double) effct_feat_num / frame_num; 1156 | 1157 | undistort_time_mean = undistort_time_mean * (frame_num - 1) / frame_num + 1158 | (undistort_time) / frame_num; 1159 | down_sample_time_mean = down_sample_time_mean * (frame_num - 1) / frame_num + 1160 | (t_downsample) / frame_num; 1161 | calc_cov_time_mean = calc_cov_time_mean * (frame_num - 1) / frame_num + 1162 | (calc_point_cov_time) / frame_num; 1163 | scan_match_time_mean = scan_match_time_mean * (frame_num - 1) / frame_num + 1164 | (scan_match_time) / frame_num; 1165 | ekf_solve_time_mean = ekf_solve_time_mean * (frame_num - 1) / frame_num + 1166 | (solve_time) / frame_num; 1167 | map_update_time_mean = map_update_time_mean * (frame_num - 1) / frame_num + 1168 | (map_incremental_time) / frame_num; 1169 | 1170 | aver_time_consu = aver_time_consu * (frame_num - 1) / frame_num + (total_time) / frame_num; 1171 | 1172 | time_log_counter++; 1173 | cout << "[ Time ]: " 1174 | << "average undistort: " << undistort_time_mean << std::endl; 1175 | cout << "[ Time ]: " 1176 | << "average down sample: " << down_sample_time_mean << std::endl; 1177 | cout << "[ Time ]: " 1178 | << "average calc cov: " << calc_cov_time_mean << std::endl; 1179 | cout << "[ Time ]: " 1180 | << "average scan match: " << scan_match_time_mean << std::endl; 1181 | cout << "[ Time ]: " 1182 | << "average solve: " << ekf_solve_time_mean << std::endl; 1183 | cout << "[ Time ]: " 1184 | << "average map incremental: " << map_update_time_mean << std::endl; 1185 | cout << "[ Time ]: " 1186 | << " average total " << aver_time_consu << endl; 1187 | 1188 | if (write_kitti_log) { 1189 | kitti_log(fp_kitti); 1190 | } 1191 | 1192 | scanIdx++; 1193 | } 1194 | status = ros::ok(); 1195 | rate.sleep(); 1196 | } 1197 | return 0; 1198 | } 1199 | --------------------------------------------------------------------------------