From fd83a80a6c70c49e03ad6a297380c946a45bb7cc Mon Sep 17 00:00:00 2001 From: Joanna-HE Date: Thu, 18 Jan 2024 09:30:28 -0500 Subject: [PATCH 1/2] point-lio-grid-map --- config/avia.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/config/avia.yaml b/config/avia.yaml index 1aa6642..ec1e95b 100755 --- a/config/avia.yaml +++ b/config/avia.yaml @@ -11,7 +11,7 @@ preprocess: lidar_type: 1 # 4 scan_line: 6 # 32 timestamp_unit: 1 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. - blind: 0.50 + blind: 1.0 mapping: imu_en: true From 08e893cda5c52c675c04908d85821af18a89adb7 Mon Sep 17 00:00:00 2001 From: Joanna-HE Date: Thu, 18 Jan 2024 10:46:16 -0500 Subject: [PATCH 2/2] fix a bug --- CMakeLists.txt | 10 ++++++++++ src/Estimator.cpp | 4 ++-- 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3055c75..3dd2365 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -75,6 +75,16 @@ link_directories( ${PCL_LIBRARY_DIRS} ) +generate_messages( + DEPENDENCIES + geometry_msgs +) + +catkin_package( + CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs message_runtime + DEPENDS EIGEN3 PCL + INCLUDE_DIRS +) add_executable(pointlio_mapping src/laserMapping.cpp src/li_initialization.cpp src/parameters.cpp src/preprocess.cpp src/Estimator.cpp src/IMU_Processing.cpp) diff --git a/src/Estimator.cpp b/src/Estimator.cpp index cda4f19..d650a62 100755 --- a/src/Estimator.cpp +++ b/src/Estimator.cpp @@ -204,7 +204,7 @@ void h_model_input(state_input &s, Eigen::Matrix3d cov_p, Eigen::Matrix3d cov_R, M3D point_crossmat = crossmat_list[idx+j+1]; V3D C(s.rot.transpose() * norm_vec); // conjugate().normalized() V3D A(point_crossmat * C); - ekfom_data.h_x.block<1, 6>(m, 0) << norm_vec(0), norm_vec(1), norm_vec(2), VEC_FROM_ARRAY(A), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; + ekfom_data.h_x.block<1, 12>(m, 0) << norm_vec(0), norm_vec(1), norm_vec(2), VEC_FROM_ARRAY(A), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; } ekfom_data.z(m) = -norm_vec(0) * feats_down_world->points[idx+j+1].x -norm_vec(1) * feats_down_world->points[idx+j+1].y -norm_vec(2) * feats_down_world->points[idx+j+1].z-normvec->points[j].intensity; @@ -310,7 +310,7 @@ void h_model_output(state_output &s, Eigen::Matrix3d cov_p, Eigen::Matrix3d cov_ M3D point_crossmat = crossmat_list[idx+j+1]; V3D C(s.rot.transpose() * norm_vec); // conjugate().normalized() V3D A(point_crossmat * C); - ekfom_data.h_x.block<1, 6>(m, 0) << norm_vec(0), norm_vec(1), norm_vec(2), VEC_FROM_ARRAY(A), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; + ekfom_data.h_x.block<1, 12>(m, 0) << norm_vec(0), norm_vec(1), norm_vec(2), VEC_FROM_ARRAY(A), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; } ekfom_data.z(m) = -norm_vec(0) * feats_down_world->points[idx+j+1].x -norm_vec(1) * feats_down_world->points[idx+j+1].y -norm_vec(2) * feats_down_world->points[idx+j+1].z-normvec->points[j].intensity;