From 9de9b4a7b94ece66561418f4f0c432a3bd8e4d43 Mon Sep 17 00:00:00 2001 From: Joanna-HE Date: Thu, 18 Jan 2024 10:36:30 -0500 Subject: [PATCH] fix a bug --- CMakeLists.txt | 16 +++++++++++++--- include/ivox/eigen_types.h | 0 include/ivox/hilbert.hpp | 0 include/ivox/ivox3d.h | 0 include/ivox/ivox3d_node.hpp | 0 include/matplotlibcpp.h | 0 launch/mapping_avia.launch | 2 +- src/Estimator.cpp | 4 ++-- 8 files changed, 16 insertions(+), 6 deletions(-) mode change 100644 => 100755 include/ivox/eigen_types.h mode change 100644 => 100755 include/ivox/hilbert.hpp mode change 100644 => 100755 include/ivox/ivox3d.h mode change 100644 => 100755 include/ivox/ivox3d_node.hpp mode change 100644 => 100755 include/matplotlibcpp.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 3055c75..09328c3 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -60,7 +60,6 @@ find_package(catkin REQUIRED COMPONENTS find_package(Eigen3 REQUIRED) find_package(PCL 1.8 REQUIRED) - message(Eigen: ${EIGEN3_INCLUDE_DIR}) include_directories( @@ -75,11 +74,22 @@ link_directories( ${PCL_LIBRARY_DIRS} ) -add_executable(pointlio_mapping src/laserMapping.cpp + +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) target_link_libraries(pointlio_mapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES}) - target_include_directories(pointlio_mapping PRIVATE ${PYTHON_INCLUDE_DIRS}) diff --git a/include/ivox/eigen_types.h b/include/ivox/eigen_types.h old mode 100644 new mode 100755 diff --git a/include/ivox/hilbert.hpp b/include/ivox/hilbert.hpp old mode 100644 new mode 100755 diff --git a/include/ivox/ivox3d.h b/include/ivox/ivox3d.h old mode 100644 new mode 100755 diff --git a/include/ivox/ivox3d_node.hpp b/include/ivox/ivox3d_node.hpp old mode 100644 new mode 100755 diff --git a/include/matplotlibcpp.h b/include/matplotlibcpp.h old mode 100644 new mode 100755 diff --git a/launch/mapping_avia.launch b/launch/mapping_avia.launch index da647d0..56b4d07 100755 --- a/launch/mapping_avia.launch +++ b/launch/mapping_avia.launch @@ -3,7 +3,7 @@ - + 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;