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:46:16 -05:00
parent fd83a80a6c
commit 08e893cda5
2 changed files with 12 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
CMakeLists.txt
View File

@@ -75,6 +75,16 @@ link_directories(
${PCL_LIBRARY_DIRS} ${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 add_executable(pointlio_mapping src/laserMapping.cpp
src/li_initialization.cpp src/parameters.cpp src/preprocess.cpp src/Estimator.cpp src/li_initialization.cpp src/parameters.cpp src/preprocess.cpp src/Estimator.cpp
src/IMU_Processing.cpp) src/IMU_Processing.cpp)

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]; M3D point_crossmat = crossmat_list[idx+j+1];
V3D C(s.rot.transpose() * norm_vec); // conjugate().normalized() V3D C(s.rot.transpose() * norm_vec); // conjugate().normalized()
V3D A(point_crossmat * C); 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; 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]; M3D point_crossmat = crossmat_list[idx+j+1];
V3D C(s.rot.transpose() * norm_vec); // conjugate().normalized() V3D C(s.rot.transpose() * norm_vec); // conjugate().normalized()
V3D A(point_crossmat * C); 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; 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;