From 08e893cda5c52c675c04908d85821af18a89adb7 Mon Sep 17 00:00:00 2001 From: Joanna-HE Date: Thu, 18 Jan 2024 10:46:16 -0500 Subject: [PATCH] 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;