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

16
CMakeLists.txt
View File

@@ -60,7 +60,6 @@ find_package(catkin REQUIRED COMPONENTS
find_package(Eigen3 REQUIRED) find_package(Eigen3 REQUIRED)
find_package(PCL 1.8 REQUIRED) find_package(PCL 1.8 REQUIRED)
message(Eigen: ${EIGEN3_INCLUDE_DIR}) message(Eigen: ${EIGEN3_INCLUDE_DIR})
include_directories( include_directories(
@@ -75,11 +74,22 @@ link_directories(
${PCL_LIBRARY_DIRS} ${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/li_initialization.cpp src/parameters.cpp src/preprocess.cpp src/Estimator.cpp
src/IMU_Processing.cpp) src/IMU_Processing.cpp)
target_link_libraries(pointlio_mapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES}) target_link_libraries(pointlio_mapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES})
target_include_directories(pointlio_mapping PRIVATE ${PYTHON_INCLUDE_DIRS}) 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

2
launch/mapping_avia.launch
View File

@@ -3,7 +3,7 @@
<arg name="rviz" default="true" /> <arg name="rviz" default="true" />
<node pkg="point_lio" type="pointlio_mapping" name="laserMapping" output="screen"> <node pkg="point_lio" type="pointlio_mapping" name="laserMapping" output="screen" >
<rosparam command="load" file="$(find point_lio)/config/avia.yaml" /> <rosparam command="load" file="$(find point_lio)/config/avia.yaml" />
<param name="use_imu_as_input" type="bool" value="0"/> <!--change to 1 to use IMU as input of Point-LIO--> <param name="use_imu_as_input" type="bool" value="0"/> <!--change to 1 to use IMU as input of Point-LIO-->
<param name="prop_at_freq_of_imu" type="bool" value="1"/> <param name="prop_at_freq_of_imu" type="bool" value="1"/>

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;