admin/Point-LIO_ROS2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix a bug

Browse Source
This commit is contained in:
Joanna-HE
2024-01-18 10:36:30 -05:00
parent d4919a8c6a
commit 9de9b4a7b9
8 changed files with 16 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
CMakeLists.txt
View File

@@ -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}
)
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})

0
include/ivox/eigen_types.h Normal file → Executable file
View File

0
include/ivox/hilbert.hpp Normal file → Executable file
View File

0
include/ivox/ivox3d.h Normal file → Executable file
View File

0
include/ivox/ivox3d_node.hpp Normal file → Executable file
View File

0
include/matplotlibcpp.h Normal file → Executable file
View File

4
src/Estimator.cpp
View File

@@ -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;