This package provides localization in a pre-built map using ICP and odometry (or the IMU measurements).

Overview

Localization using ICP in a known map

Overview

This package localizes the lidar sensor in a given map using the ICP algorithm. It subscribes to lidar scans and it registers them in a given map. If provided, it can use odometry or IMU to extrapolate the pose between the two iterations of ICP.

The user should provide a reference map (point cloud) as as a .pcd file and an initial pose in the reference map.

Released under BSD 3-Clause license. Parts of the code in this repo have been inspired by the code inside google cartogrpaher. We've retained the copyright headers where applicable.

Author: Edo Jelavic

Maintainer: Edo Jelavic, [email protected]

localization in a forest localization in an urban environment
forest urban

The package has been tested on the platforms shown in the images below. It has also been used during the ETH Robotic Summer School 2021 (link).

handheld sensor module skid steer robot legged robot
handheld skid-steer anymal

Installation

Clone the following three dependencies:

# in your source folder `src`
git clone https://github.com/leggedrobotics/libnabo.git
git clone https://github.com/leggedrobotics/libpointmatcher.git
git clone https://github.com/leggedrobotics/pointmatcher-ros.git

Install ROS and library dependencies with:

sudo apt install -y ros-noetic-pcl-ros ros-noetic-pcl-conversions ros-noetic-eigen-conversions ros-noetic-tf-conversions ros-noetic-tf2-geometry libgoogle-glog-dev
# OR, use rosdep in your source folder `src` 
sudo rosdep install -yr --from-paths .

Recommended to build in release mode for performance (catkin config -DCMAKE_BUILD_TYPE=Release)

Build with:

catkin build icp_localization

Usage

This package is based on the libpointmatcher package and it uses the ICP implementation from there. Libpointmatcher has an extensive documentation. icp_localization provides ROS wrappers and uses either odometry or IMU measurements to calculate initial guesses for the pointcloud alignment.

You can launch the program on the robot with: roslaunch icp_localization icp_node.launch. The pcd_filepath parameter in the launch file should point to the location where you stored your refrence map (pointcloud) in the .pcd format.

You can download the example bags and the example config files here. You can copy paste the rosbag and the map (.pcd file) to the data folder. Put the .yaml file in the config folder and you should be ready to run the forest environment example. For running the urban example, please adjust the parameters in the icp_node_rosbag.launch file. You need to chnage the pcd_filename, input_filters_config_name, bag_filename and the parameter_filepath.

The rosbag examples can be luaunched with roslaunch icp_localization icp_node_rosbag.launch

Note that the urban dataset uses the velodyne LIDAR whereas the forest dataset uses the ouster LIDAR. Please adjust the input_filters config file accordingly. Furthermore, in the forest dataset instead of full scans, each lidar packet is converted to pointcloud msg and then published.

The system has been tested with T265 tracking camera as an odometry source as well as legged odometry. If you are using the T265 make sure that you disable internal mapping and localization since it can cause pose jumps which will break the ICP.

The node also subscribes to the /initialpose topic and you can use rviz to set the initial pose of the range sensor. Note that this is not the initial robot pose since the range sensor coordinate frame might not coincide with the robot frame.

The node publishes the TF tree: map->odom->odom_source->range_sensor (in case you are using the odometry).

Configuration

The configuration is split into three .yaml files.

The icp.yaml file configures the ICP settings such as error metric and outlier filters. Any filter that is applied to the map can also be defined here.

The input_filters.yaml file configures operations that are applied to each scan of the range sensors. Subsampling, cropping and normal computation are configured in this file. Two examples have been provided (one for the velodyne puck range sensor and the other one for the ouste OS1 sensor).

The filtering and the ICP can be configured by adding your own custom configuration .yaml files. Documentation on how to do that can be found here.

The rest of the parameters is explained below:

  • icp_localization/initial_pose - initial pose of the range sensor frame in the provided map.
  • icp_localization/imu_data_topic - ROS topic on which the imu data is published
  • icp_localization/odometry_data_topic - ROS topic on which the odometry data is published
  • icp_localization/num_accumulated_range_data - Number of pointcloud messages that will be accumulated before trying to register them in a map. In case you are using full scans this parameter should be set to 1. In case you are publishing LIDAR packets, you need to convert them to sensor_msgs::Pointcloud2 first. At the moment there is no motion compensation implemented.
  • icp_localization/range_data_topic - ROS topic on which the LIDAR data is published
  • icp_localization/is_use_odometry - Whether to use odometry for initial pose prediction. If set the false, the pose extrapolator will try to use the imu data.
  • icp_localization/is_provide_odom_frame - Whether to provide the odom frame or publish directly map to range sensor transformation only
  • icp_localization/gravity_vector_filter_time_constant - Constant used for filtering imu measurements when estimating gravity. Smaller constant gives noisier estimates but adapts quicker to changes in orientation. Higher numbers give smoother estimates but take longer time to adapt to new orientation.
  • icp_localization/fixed_frame - Fixed frame map. Used mostly for visualization.
  • icp_localization/min_num_odom_msgs_before_ready - Ensure to have minimum number of msgs before starting ICP such that we can interpolate between them.
  • calibration - calibration parameters between sensors. The coordinate frames are shown below. Tos_rs is a transoformation from odometry source (e.g. tracking camera) to the range sensor frame.Timu_rs is the transformaion form the imu to the range sensor frame.

frames

All coordinate frames follow the URDF convention: http://wiki.ros.org/urdf/XML/joint.

Comments
  • ICP localization doesn't work as expected

    ICP localization doesn't work as expected

    I'm testing this package to localize my mobile base on a map. I have used AMCL, but I'm struggling with a jumping pose estimation. I had hopes that this package would resolve the problem, but I'm having some problems. My mobile base has bad odometry, so I would expect a correction because of a match between the scan (in point cloud format) and the map (in point cloud format). What I see is no correction, and I can understand why. The map was made using gmapping and later transformed into a point cloud using a simple script I have made.

    The result I have: screenshot

    opened by JackFrost67 5
  • Slam for map generation (ICP based)

    Slam for map generation (ICP based)

    Hello, I would like to use this package on my ICP based project. I know this is not a good place for this questions but, can you suggest me a good 3D ICP based slam to use with this one?

    question 
    opened by Cristian-wp 2
  • err:PluginlibFactory: The plugin for class 'jsk_rviz_plugin/TFTrajectory' failed to load ,when roslaunch icp_localization icp_node_rosbag.launch

    err:PluginlibFactory: The plugin for class 'jsk_rviz_plugin/TFTrajectory' failed to load ,when roslaunch icp_localization icp_node_rosbag.launch

    [ERROR] [1664349017.901695003, 1625818819.138769297]: PluginlibFactory: The plugin for class 'jsk_rviz_plugin/TFTrajectory' failed to load. Error: According to the loaded plugin descriptions the class jsk_rviz_plugin/TFTrajectory with base class type rviz::Display does not exist. Declared types are moveit_rviz_plugin/MotionPlanning moveit_rviz_plugin/PlanningScene moveit_rviz_plugin/RobotState moveit_rviz_plugin/Trajectory rviz/AccelStamped rviz/Axes rviz/Camera rviz/DepthCloud rviz/Effort rviz/FluidPressure rviz/Grid rviz/GridCells rviz/Illuminance rviz/Image rviz/InteractiveMarkers rviz/LaserScan rviz/Map rviz/Marker rviz/MarkerArray rviz/Odometry rviz/Path rviz/PointCloud rviz/PointCloud2 rviz/PointStamped rviz/Polygon rviz/Pose rviz/PoseArray rviz/PoseWithCovariance rviz/Range rviz/RelativeHumidity rviz/RobotModel rviz/TF rviz/Temperature rviz/TwistStamped rviz/WrenchStamped rviz_plugin_tutorials/Imu

    opened by dbdxnuliba 2
  • Dose this project include distortion correction function?

    Dose this project include distortion correction function?

    Hi, thank you for your great work about icp_localization. I want to use it for my robot and my lidar is velodyne-16. When my robot run fast, such as 1m/s, the lidar pointcloud will be affected. So I want to know whether icp_localization have pointcloud distortion correction function. Or, I need to write it by myself.

    opened by JACKLiuDay 2
  • ERROR: cannot launch node of type [icp_localization/s_localizer_node]:

    ERROR: cannot launch node of type [icp_localization/s_localizer_node]:

    Hi,

    Thank you for this very useful package. I tried running the icp_node.launch but I get the following error below. I compiled the package with "catkin_make_isolated" command and I can see that the "localizer_node" is added as executable in the Cmakelists.txt file .

    Can you please help me with error ?

    Many Thanks.

    ERROR: cannot launch node of type [icp_localization/s_localizer_node]: can't locate node [s_localizer_node] in package [icp_localization]

    "Build" section from CmakeLists.txt file:

    ###########

    Build

    ###########

    include_directories( include SYSTEM ${libpointmatcher_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} )

    link_directories( #only needed for pcl ${PCL_LIBRARY_DIRS} )

    add_definitions( #only needed for pcl ${PCL_DEFINITIONS} )

    add_library(${PROJECT_NAME} ${SRC_FILES} )

    target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${LIBPOINTMATCHER_LIBRARIES} ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES} )

    add_executable(localizer_node src/localizer_node.cpp )

    target_link_libraries(localizer_node ${PROJECT_NAME} ${catkin_LIBRARIES} ${libpointmatcher_LIBRARIES} ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES} )

    opened by barkindagda 1
  • Robustness in dynamic environment

    Robustness in dynamic environment

    Hi, thank you for sharing this great work. I'm wondering if the localization can work reliably in an indoor dynamic environment (like a warehouse with shelves and aisles but the goods/pallets on shelves always change). Thank you!

    question 
    opened by bxc237 1
  • [icp_node-2] process has died [pid 14603, exit code -6

    [icp_node-2] process has died [pid 14603, exit code -6

    Hi,

    Anyone know how to solve this error ?

    localizer_node: /usr/include/eigen3/Eigen/src/Core/DenseCoeffsBase.h:118: Eigen::DenseCoeffsBase<Derived, 0>::CoeffReturnType Eigen::DenseCoeffsBase<Derived, 0>::operator()(Eigen::Index, Eigen::Index) const [with Derived = Eigen::Matrix<float, -1, -1>; Eigen::DenseCoeffsBase<Derived, 0>::CoeffReturnType = const float&; Eigen::Index = long int]: Assertion `row >= 0 && row < rows() && col >= 0 && col < cols()' failed. No handlers could be found for logger "roslaunch"

    [icp_node-2] process has died [pid 14603, exit code -6

    Thank you.

    opened by barkindagda 6
  • Failed     << pointmatcher_ros:make                [ Exited with code 2 ]

    Failed << pointmatcher_ros:make [ Exited with code 2 ]

    Thank you very much for your work. I have this problem when running catkin build icp_localization, and it seems to remain me that there are some system files that were missing, not sure where to go...

    Here's part of the output information, running on Ubuntu20.04, and file path in: ~/carto_ws/src/icp_localization ~/carto_ws/src/libnabo ~/carto_ws/src/libpointmatcher ~/carto_ws/src/pointmatcher-ros Terminal Output: `.......................................................................................... Finished <<< libpointmatcher [ 1 minute and 6.1 seconds ]
    Starting >>> pointmatcher_ros


    Warnings << pointmatcher_ros:cmake /home/sim2real/carto_ws/logs/pointmatcher_ros/build.cmake.000.log CMake Warning at /opt/cmake/share/cmake-3.14/Modules/FindBoost.cmake:896 (message): New Boost version may have incorrect or missing dependencies and imported targets Call Stack (most recent call first): /opt/cmake/share/cmake-3.14/Modules/FindBoost.cmake:1018 (_Boost_COMPONENT_DEPENDENCIES) /opt/cmake/share/cmake-3.14/Modules/FindBoost.cmake:1694 (_Boost_MISSING_DEPENDENCIES) /usr/lib/x86_64-linux-gnu/cmake/pcl/PCLConfig.cmake:134 (find_package) /usr/lib/x86_64-linux-gnu/cmake/pcl/PCLConfig.cmake:314 (find_boost) /usr/lib/x86_64-linux-gnu/cmake/pcl/PCLConfig.cmake:551 (find_external_library) CMakeLists.txt:30 (find_package)

    CMake Warning at /opt/cmake/share/cmake-3.14/Modules/FindBoost.cmake:896 (message): New Boost version may have incorrect or missing dependencies and imported targets Call Stack (most recent call first): /opt/cmake/share/cmake-3.14/Modules/FindBoost.cmake:1018 (_Boost_COMPONENT_DEPENDENCIES) /opt/cmake/share/cmake-3.14/Modules/FindBoost.cmake:1694 (_Boost_MISSING_DEPENDENCIES) /usr/lib/x86_64-linux-gnu/cmake/pcl/PCLConfig.cmake:134 (find_package) /usr/lib/x86_64-linux-gnu/cmake/pcl/PCLConfig.cmake:314 (find_boost) /usr/lib/x86_64-linux-gnu/cmake/pcl/PCLConfig.cmake:551 (find_external_library) CMakeLists.txt:30 (find_package)

    CMake Warning at /opt/cmake/share/cmake-3.14/Modules/FindBoost.cmake:896 (message): New Boost version may have incorrect or missing dependencies and imported targets Call Stack (most recent call first): /opt/cmake/share/cmake-3.14/Modules/FindBoost.cmake:1018 (_Boost_COMPONENT_DEPENDENCIES) /opt/cmake/share/cmake-3.14/Modules/FindBoost.cmake:1694 (_Boost_MISSING_DEPENDENCIES) /usr/lib/x86_64-linux-gnu/cmake/pcl/PCLConfig.cmake:134 (find_package) /usr/lib/x86_64-linux-gnu/cmake/pcl/PCLConfig.cmake:314 (find_boost) /usr/lib/x86_64-linux-gnu/cmake/pcl/PCLConfig.cmake:551 (find_external_library) CMakeLists.txt:30 (find_package)

    CMake Warning at /opt/cmake/share/cmake-3.14/Modules/FindBoost.cmake:896 (message): New Boost version may have incorrect or missing dependencies and imported targets Call Stack (most recent call first): /opt/cmake/share/cmake-3.14/Modules/FindBoost.cmake:1018 (_Boost_COMPONENT_DEPENDENCIES) /opt/cmake/share/cmake-3.14/Modules/FindBoost.cmake:1694 (_Boost_MISSING_DEPENDENCIES) /usr/lib/x86_64-linux-gnu/cmake/pcl/PCLConfig.cmake:134 (find_package) /usr/lib/x86_64-linux-gnu/cmake/pcl/PCLConfig.cmake:314 (find_boost) /usr/lib/x86_64-linux-gnu/cmake/pcl/PCLConfig.cmake:551 (find_external_library) CMakeLists.txt:30 (find_package)

    CMake Warning at /opt/cmake/share/cmake-3.14/Modules/FindBoost.cmake:896 (message): New Boost version may have incorrect or missing dependencies and imported targets Call Stack (most recent call first): /opt/cmake/share/cmake-3.14/Modules/FindBoost.cmake:1018 (_Boost_COMPONENT_DEPENDENCIES) /opt/cmake/share/cmake-3.14/Modules/FindBoost.cmake:1694 (_Boost_MISSING_DEPENDENCIES) /usr/lib/x86_64-linux-gnu/cmake/pcl/PCLConfig.cmake:134 (find_package) /usr/lib/x86_64-linux-gnu/cmake/pcl/PCLConfig.cmake:314 (find_boost) /usr/lib/x86_64-linux-gnu/cmake/pcl/PCLConfig.cmake:551 (find_external_library) CMakeLists.txt:30 (find_package)

    ** WARNING ** io features related to pcap will be disabled ** WARNING ** io features related to png will be disabled ** WARNING ** io features related to libusb-1.0 will be disabled cd /home/sim2real/carto_ws/build/pointmatcher_ros; catkin build --get-env pointmatcher_ros | catkin env -si /usr/local/bin/cmake /home/sim2real/carto_ws/src/pointmatcher-ros --no-warn-unused-cli -DCATKIN_DEVEL_PREFIX=/home/sim2real/carto_ws/devel/.private/pointmatcher_ros -DCMAKE_INSTALL_PREFIX=/home/sim2real/carto_ws/install; cd -

    ..........................................................................................


    Errors << pointmatcher_ros:make /home/sim2real/carto_ws/logs/pointmatcher_ros/build.make.000.log In file included from /usr/include/c++/9/complex:44, from /usr/include/eigen3/Eigen/Core:96, from /usr/include/eigen3/Eigen/StdVector:14, from /home/sim2real/carto_ws/src/libpointmatcher/pointmatcher/PointMatcher.h:47, from /home/sim2real/carto_ws/src/pointmatcher-ros/include/pointmatcher_ros/serialization.h:5, from /home/sim2real/carto_ws/src/pointmatcher-ros/src/serialization.cpp:3: /usr/include/c++/9/cmath:45:15: fatal error: math.h: No such file or directory 45 | #include_next <math.h> | ^~~~~~~~ In file included from /usr/include/c++/9/complex:44, from /usr/include/eigen3/Eigen/Core:96, from /usr/include/eigen3/Eigen/StdVector:14, from /home/sim2real/carto_ws/src/libpointmatcher/pointmatcher/PointMatcher.h:47, from /home/sim2real/carto_ws/src/pointmatcher-ros/include/pointmatcher_ros/deserialization.h:5, from /home/sim2real/carto_ws/src/pointmatcher-ros/src/deserialization.cpp:3: /usr/include/c++/9/cmath:45:15: fatal error: math.h: No such file or directory 45 | #include_next <math.h> | ^~~~~~~~ compilation terminated. compilation terminated. In file included from /usr/include/c++/9/complex:44, from /usr/include/eigen3/Eigen/Core:96, from /usr/include/eigen3/Eigen/Dense:1, from /usr/include/eigen3/Eigen/Eigen:1, from /home/sim2real/carto_ws/src/pointmatcher-ros/include/pointmatcher_ros/transform.h:5, from /home/sim2real/carto_ws/src/pointmatcher-ros/src/transform.cpp:2: /usr/include/c++/9/cmath:45:15: fatal error: math.h: No such file or directory 45 | #include_next <math.h> | ^~~~~~~~ compilation terminated. make[2]: *** [CMakeFiles/pointmatcher_ros.dir/build.make:76: CMakeFiles/pointmatcher_ros.dir/src/deserialization.cpp.o] Error 1 make[2]: *** Waiting for unfinished jobs.... make[2]: *** [CMakeFiles/pointmatcher_ros.dir/build.make:89: CMakeFiles/pointmatcher_ros.dir/src/serialization.cpp.o] Error 1 make[2]: *** [CMakeFiles/pointmatcher_ros.dir/build.make:102: CMakeFiles/pointmatcher_ros.dir/src/transform.cpp.o] Error 1 In file included from /usr/include/c++/9/ext/string_conversions.h:41, from /usr/include/c++/9/bits/basic_string.h:6493, from /usr/include/c++/9/string:55, from /opt/ros/noetic/include/ros/platform.h:38, from /opt/ros/noetic/include/ros/time.h:53, from /opt/ros/noetic/include/ros/ros.h:38, from /home/sim2real/carto_ws/src/pointmatcher-ros/include/pointmatcher_ros/helper_functions.h:4, from /home/sim2real/carto_ws/src/pointmatcher-ros/src/helper_functions.cpp:2: /usr/include/c++/9/cstdlib:75:15: fatal error: stdlib.h: No such file or directory 75 | #include_next <stdlib.h> | ^~~~~~~~~~ compilation terminated. make[2]: *** [CMakeFiles/pointmatcher_ros.dir/build.make:63: CMakeFiles/pointmatcher_ros.dir/src/helper_functions.cpp.o] Error 1 In file included from /usr/include/c++/9/complex:44, from /usr/include/eigen3/Eigen/Core:96, from /usr/include/eigen3/Eigen/StdVector:14, from /home/sim2real/carto_ws/src/libpointmatcher/pointmatcher/PointMatcher.h:47, from /home/sim2real/carto_ws/src/libpointmatcher/pointmatcher/IO.h:39, from /home/sim2real/carto_ws/src/pointmatcher-ros/include/pointmatcher_ros/RosPointCloud2Deserializer.h:7, from /home/sim2real/carto_ws/src/pointmatcher-ros/src/RosPointCloud2Deserializer.cpp:2: /usr/include/c++/9/cmath:45:15: fatal error: math.h: No such file or directory 45 | #include_next <math.h> | ^~~~~~~~ compilation terminated. make[2]: *** [CMakeFiles/pointmatcher_ros.dir/build.make:141: CMakeFiles/pointmatcher_ros.dir/src/RosPointCloud2Deserializer.cpp.o] Error 1 In file included from /usr/include/c++/9/ext/string_conversions.h:41, from /usr/include/c++/9/bits/basic_string.h:6493, from /usr/include/c++/9/string:55, from /home/sim2real/carto_ws/src/pointmatcher-ros/include/pointmatcher_ros/PmTf.h:4, from /home/sim2real/carto_ws/src/pointmatcher-ros/src/PmTf.cpp:2: /usr/include/c++/9/cstdlib:75:15: fatal error: stdlib.h: No such file or directory 75 | #include_next <stdlib.h> | ^~~~~~~~~~ compilation terminated. make[2]: *** [CMakeFiles/pointmatcher_ros.dir/build.make:115: CMakeFiles/pointmatcher_ros.dir/src/PmTf.cpp.o] Error 1 In file included from /usr/include/c++/9/ext/string_conversions.h:41, from /usr/include/c++/9/bits/basic_string.h:6493, from /usr/include/c++/9/string:55, from /usr/include/c++/9/bits/locale_classes.h:40, from /usr/include/c++/9/bits/ios_base.h:41, from /usr/include/c++/9/ios:42, from /usr/include/c++/9/istream:38, from /usr/include/c++/9/sstream:38, from /opt/ros/noetic/include/ros/console.h:38, from /home/sim2real/carto_ws/src/pointmatcher-ros/include/pointmatcher_ros/PointMatcherFilterInterface.h:4, from /home/sim2real/carto_ws/src/pointmatcher-ros/src/PointMatcherFilterInterface.cpp:2: /usr/include/c++/9/cstdlib:75:15: fatal error: stdlib.h: No such file or directory 75 | #include_next <stdlib.h> | ^~~~~~~~~~ compilation terminated. make[2]: *** [CMakeFiles/pointmatcher_ros.dir/build.make:128: CMakeFiles/pointmatcher_ros.dir/src/PointMatcherFilterInterface.cpp.o] Error 1 In file included from /usr/include/c++/9/ext/string_conversions.h:41, from /usr/include/c++/9/bits/basic_string.h:6493, from /usr/include/c++/9/string:55, from /usr/include/c++/9/stdexcept:39, from /usr/include/c++/9/array:39, from /usr/include/c++/9/tuple:39, from /usr/include/c++/9/bits/unique_ptr.h:37, from /usr/include/c++/9/memory:80, from /home/sim2real/carto_ws/src/pointmatcher-ros/include/pointmatcher_ros/StampedPointCloud.h:4, from /home/sim2real/carto_ws/src/pointmatcher-ros/src/StampedPointCloud.cpp:2: /usr/include/c++/9/cstdlib:75:15: fatal error: stdlib.h: No such file or directory 75 | #include_next <stdlib.h> | ^~~~~~~~~~ compilation terminated. make[2]: *** [CMakeFiles/pointmatcher_ros.dir/build.make:154: CMakeFiles/pointmatcher_ros.dir/src/StampedPointCloud.cpp.o] Error 1 make[1]: *** [CMakeFiles/Makefile2:1899: CMakeFiles/pointmatcher_ros.dir/all] Error 2 make: *** [Makefile:141: all] Error 2 cd /home/sim2real/carto_ws/build/pointmatcher_ros; catkin build --get-env pointmatcher_ros | catkin env -si /usr/bin/make --jobserver-auth=3,4; cd -

    .......................................................................................... Failed << pointmatcher_ros:make [ Exited with code 2 ]
    Failed <<< pointmatcher_ros [ 2.5 seconds ]
    Abandoned <<< icp_localization [ Unrelated job failed ]
    [build] Summary: 3 of 5 packages succeeded.
    [build] Ignored: 4 packages were skipped or are skiplisted.
    [build] Warnings: 2 packages succeeded with warnings.
    [build] Abandoned: 1 packages were abandoned.
    [build] Failed: 1 packages failed.
    [build] Runtime: 1 minute and 17.4 seconds total.
    [build] Note: Workspace packages have changed, please re-source setup files to use them.`

    opened by AndrewWu1998 1
  •  received signal SIGSEGV, Segmentation fault

    received signal SIGSEGV, Segmentation fault

    Thank you very much for your work. I have this problem when using libpointmatcher and spent a long time to solve this problem, but there is still no result.

    `[Thread debugging using libthread_db enabled] Using host libthread_db library "/lib/x86_64-linux-gnu/libthread_db.so.1". [New Thread 0x7fffe290c700 (LWP 6544)] [New Thread 0x7fffe210b700 (LWP 6545)] [New Thread 0x7fffe190a700 (LWP 6546)] [New Thread 0x7fffe1109700 (LWP 6551)] [New Thread 0x7fffe0908700 (LWP 6582)] [New Thread 0x7fffd3fff700 (LWP 6583)]

    Thread 1 "localizer_node" received signal SIGSEGV, Segmentation fault. 0x00007ffff7bb0515 in void Eigen::internal::call_dense_assignment_loop<Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::internal::assign_op<float, float> >(Eigen::Matrix<float, -1, -1, 0, -1, -1>&, Eigen::Matrix<float, -1, -1, 0, -1, -1> const&, Eigen::internal::assign_op<float, float> const&) () from /home/yuan/icp_localization_ws/devel/.private/icp_localization/lib/libicp_localization.so (gdb) [icp_node-2] killing on exit Quit (gdb) bt #0 0x00007ffff7bb0515 in void Eigen::internal::call_dense_assignment_loop<Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::internal::assign_op<float, float> >(Eigen::Matrix<float, -1, -1, 0, -1, -1>&, Eigen::Matrix<float, -1, -1, 0, -1, -1> const&, Eigen::internal::assign_op<float, float> const&) () from /home/yuan/icp_localization_ws/devel/.private/icp_localization/lib/libicp_localization.so #1 0x00007ffff5947b15 in SamplingSurfaceNormalDataPointsFilter::fuseRange(SamplingSurfaceNormalDataPointsFilter::BuildData&, int, int) const () from /home/yuan/icp_localization_ws/devel/.private/libpointmatcher/lib/libpointmatcher.so #2 0x00007ffff5949e15 in SamplingSurfaceNormalDataPointsFilter::buildNew(SamplingSurfaceNormalDataPointsFilter::BuildData&, int, int, Eigen::Matrix<float, -1, 1, 0, -1, 1>&&, Eigen::Matrix<float, -1, 1, 0, -1, 1>&&) const () from /home/yuan/icp_localization_ws/devel/.private/libpointmatcher/lib/libpointmatcher.so #3 0x00007ffff594a2ba in SamplingSurfaceNormalDataPointsFilter::buildNew(SamplingSurfaceNormalDataPointsFilter::BuildData&, int, int, Eigen::Matrix<float, -1, 1, 0, -1, 1>&&, Eigen::Matrix<float, -1, 1, 0, -1, 1>&&) const () from /home/yuan/icp_localization_ws/devel/.private/libpointmatcher/lib/libpointmatcher.so #4 0x00007ffff594a2ba in SamplingSurfaceNormalDataPointsFilter::buildNew(SamplingSurfaceNormalDataPointsFilter::BuildData&, int, int, Eigen::Matrix<float, -1, 1, 0, -1, 1>&&, Eigen::Matrix<float, -1, 1, 0, -1, 1>&&) const () from /home/yuan/icp_localization_ws/devel/.private/libpointmatcher/lib/libpointmatcher.so #5 0x00007ffff594a2ba in SamplingSurfaceNormalDataPointsFilter::buildNew(SamplingSurfaceNormalDataPointsFilter::BuildData&, int, int, Eigen::Matrix<float, -1, 1, 0, -1, 1>&&, Eigen::Matrix<float, -1, 1, 0, -1, 1>&&) const () from /home/yuan/icp_localization_ws/devel/.private/libpointmatcher/lib/libpointmatcher.so #6 0x00007ffff594a2ba in SamplingSurfaceNormalDataPointsFilter::buildNew(SamplingSurfaceNormalDataPointsFilter::BuildData&, int, int, Eigen::Matrix<float, -1, 1, 0, -1, 1>&&, Eigen::Matrix<float, -1, 1, 0, -1, 1>&&) const () from /home/yuan/icp_localization_ws/devel/.private/libpointmatcher/lib/libpointmatcher.so #7 0x00007ffff594a2ba in SamplingSurfaceNormalDataPointsFilter::buildNew(SamplingSurfaceNormalDataPointsFilter::BuildData&, int, int, Eigen::Matrix<float, -1, 1, 0, -1, 1>&&, Eigen::Matrix<float, -1, 1, 0, -1, 1>&&) const () from /home/yuan/icp_localization_ws/devel/.private/libpointmatcher/lib/libpointmatcher.so #8 0x00007ffff594a2ba in SamplingSurfaceNormalDataPointsFilter::buildNew(SamplingSurfaceNormalDataPointsFilter::BuildData&, int, int, Eigen::Matrix<float, -1, 1, 0, -1, 1>&&, Eigen::Matrix<float, -1, 1, 0, -1, 1>&&) const () from /home/yuan/icp_localization_ws/devel/.private/libpointmatcher/lib/libpointmatcher.so #9 0x00007ffff594a2ba in SamplingSurfaceNormalDataPointsFilter::buildNew(SamplingSurfaceNormalDataPointsFilter::BuildData&, int, int, Eigen::Matrix<float, -1, 1, 0, -1, 1>&&, Eigen::Matrix<float, -1, 1, 0, -1, 1>&&) const () from /home/yuan/icp_localization_ws/devel/.private/libpointmatcher/lib/libpointmatcher.so #10 0x00007ffff594a2ba in SamplingSurfaceNormalDataPointsFilter::buildNew(SamplingSurfaceNormalDataPointsFilter::BuildData&, int, int, Eigen::Matrix<float, -1, 1, 0, -1, 1>&&, Eigen::Matrix<float, -1, 1, 0, -1, 1>&&) const () from /home/yuan/icp_localization_ws/devel/.private/libpointmatcher/lib/libpointmatcher.so #11 0x00007ffff594a2ba in SamplingSurfaceNormalDataPointsFilter::buildNew(SamplingSurfaceNormalDataPointsFilter::BuildData&, int, int, Eigen::Matrix<float, -1, 1, 0, -1, 1>&&, Eigen::Matrix<float, -1, 1, 0, -1, 1>&&) const () from /home/yuan/icp_localization_ws/devel/.private/libpointmatcher/lib/libpointmatcher.so #12 0x00007ffff594a2ba in SamplingSurfaceNormalDataPointsFilter::buildNew(SamplingSurfaceNormalDataPointsFilter::BuildData&, int, int, Eigen::Matrix<float, -1, 1, 0, -1, 1>&&, Eigen::Matrix<float, -1, 1, 0, -1, 1>&&) const () from /home/yuan/icp_localization_ws/devel/.private/libpointmatcher/lib/libpointmatcher.so #13 0x00007ffff594a2ba in SamplingSurfaceNormalDataPointsFilter::buildNew(SamplingSurfaceNormalDataPointsFilter::BuildData&, int, int, Eigen::Matrix<float, -1, 1, 0, -1, 1>&&, Eigen::Matrix<float, -1, 1, 0, -1, 1>&&) const () from /home/yuan/icp_localization_ws/devel/.private/libpointmatcher/lib/libpointmatcher.so #14 0x00007ffff594a2ba in SamplingSurfaceNormalDataPointsFilter::buildNew(SamplingSurfaceNormalDataPointsFilter::BuildData&, int, int, Eigen::Matrix<float, -1, 1, 0, -1, 1>&&, Eigen::Matrix<float, -1, 1, 0, -1, 1>&&) const () from /home/yuan/icp_localization_ws/devel/.private/libpointmatcher/lib/libpointmatcher.so #15 0x00007ffff594a2ba in SamplingSurfaceNormalDataPointsFilter::buildNew(SamplingSurfaceNormalDataPointsFilter::BuildData&, int, int, Eigen::Matrix<float, -1, 1, 0, -1, 1>&&, Eigen::Matrix<float, -1, 1, 0, -1, 1>&&) const () from /home/yuan/icp_localization_ws/devel/.private/libpointmatcher/lib/libpointmatcher.so #16 0x00007ffff594a2ba in SamplingSurfaceNormalDataPointsFilter::buildNew(SamplingSurfaceNormalDataPointsFilter::BuildData&, int, int, Eigen::Matrix<float, -1, 1, 0, -1, 1>&&, Eigen::Matrix<float, -1, 1, 0, -1, 1>&&) const () from /home/yuan/icp_localization_ws/devel/.private/libpointmatcher/lib/libpointmatcher.so #17 0x00007ffff594a2ba in SamplingSurfaceNormalDataPointsFilter::buildNew(SamplingSurfaceNormalDataPointsFilter::BuildData&, int, int, Eigen::Matrix<float, -1, 1, 0, -1, 1>&&, Eigen::Matrix<float, -1, 1, 0, -1, 1>&&) const () from /home/yuan/icp_localization_ws/devel/.private/libpointmatcher/lib/libpointmatcher.so #18 0x00007ffff594a2ba in SamplingSurfaceNormalDataPointsFilter::buildNew(SamplingSurfaceNormalDataPointsFilter::BuildData&, int, int, Eigen::Matrix<float, -1, 1, 0, -1, 1>&&, Eigen::Matrix<float, -1, 1, 0, -1, 1>&&) const () from /home/yuan/icp_localization_ws/devel/.private/libpointmatcher/lib/libpointmatcher.so #19 0x00007ffff594a2ba in SamplingSurfaceNormalDataPointsFilter::buildNew(SamplingSurfaceNormalDataPointsFilter::BuildData&, int, int, Eigen::Matrix<float, -1, 1, 0, -1, 1>&&, Eigen::Matrix<float, -1, 1, 0, -1, 1>&&) const () from /home/yuan/icp_localization_ws/devel/.private/libpointmatcher/lib/libpointmatcher.so #20 0x00007ffff594a2ba in SamplingSurfaceNormalDataPointsFilter::buildNew(SamplingSurfaceNormalDataPointsFilter::BuildData&, int, int, Eigen::Matrix<float, -1, 1, 0, -1, 1>&&, Eigen::Matrix<float, -1, 1, 0, -1, 1>&&) const () from /home/yuan/icp_localization_ws/devel/.private/libpointmatcher/lib/libpointmatcher.so #21 0x00007ffff594a2ba in SamplingSurfaceNormalDataPointsFilter::buildNew(SamplingSurfaceNormalDataPointsFilter::BuildData&, int, int, Eigen::Matrix<float, -1, 1, 0, -1, 1>&&, Eigen::Matrix<float, -1, 1, 0, -1, 1>&&) const () from /home/yuan/icp_localization_ws/devel/.private/libpointmatcher/lib/libpointmatcher.so `

    opened by zhangyuan12121 6
Owner
Robotic Systems Lab - Legged Robotics at ETH Zürich
The Robotic Systems Lab investigates the development of machines and their intelligence to operate in rough and challenging environments.
Robotic Systems Lab - Legged Robotics at ETH Zürich
Direct LiDAR Odometry: Fast Localization with Dense Point Clouds

Direct LiDAR Odometry: Fast Localization with Dense Point Clouds DLO is a lightweight and computationally-efficient frontend LiDAR odometry solution w

VECTR at UCLA 369 Dec 30, 2022
Implementation of Monocular Direct Sparse Localization in a Prior 3D Surfel Map (DSL)

Implementation of Monocular Direct Sparse Localization in a Prior 3D Surfel Map (DSL)

Haoyang Ye 93 Nov 30, 2022
A simple localization framework that can re-localize in one point-cloud map.

Livox-Localization This repository implements a point-cloud map based localization framework. The odometry information is published with FAST-LIO. And

Siyuan Huang 94 Jan 2, 2023
pre-built coreboot images and documentation on how to flash them for Thinkpad Laptops

Skulls - not quite Heads pre-built coreboot images with an easy installation process Skulls makes it easy to install an unlocked, up-to-date and easy

Martin Kepplinger 514 Jan 3, 2023
A ros package for robust odometry and mapping using LiDAR with aid of different sensors

W-LOAM A ros package for robust odometry and mapping using LiDAR with aid of different sensors Demo Video https://www.bilibili.com/video/BV1Fy4y1L7kZ?

Saki-Chen 51 Nov 2, 2022
A pre-boot execution environment for Apple boards built on top of checkra1n

archOS A pre-boot execution environment for Apple boards built on top of checkra1n - currently based off the Checkra1n/PongoOS Repo. Building on macOS

ScarletAI 2 Jan 17, 2022
An implementation of ICP(Iterative Closest Point).

An implementation of ICP(Iterative Closest Point). Users can specify which dimensions of transformation to optimize.

Ming Cao 5 Aug 18, 2022
The ROS version of ICP Mapping with QPEP Solver (Quadratic Pose Estimation Problems)

The ROS version of ICP Mapping with QPEP Solver (Quadratic Pose Estimation Problems) The project is based on https://github.com/ethz-asl/ethzasl_icp_m

Jin Wu 11 Nov 2, 2022
A ROS package for mobile robot localization with 2D LiDAR

mcl_ros About mcl_ros mcl_ros is a ROS package for mobile robot localization with 2D LiDAR. To implement localization, Monte Carlo localization (MCL)

Naoki Akai 47 Oct 13, 2022
A fantasy map generator based on Martin O'Leary's "Generating fantasy map" notes

Fantasy Map Generator This program is an implementation of a fantasy map generator written in C++ based on the methods described in Martin O'Leary's "

Ryan Guy 630 Dec 16, 2022
A lightweight C++14 parsing library for tmx map files created with the Tiled map editor

tmxlite Description A lightweight C++14 parsing library for tmx map files created with the Tiled map editor. Requires no external linking, all depende

Matt Styles 325 Dec 26, 2022
A run-time C++ library for working with units of measurement and conversions between them and with string representations of units and measurements

Units What's new Some of the CMake target names have changed in the latest release, please update builds appropriately Documentation A library that pr

Lawrence Livermore National Laboratory 112 Dec 14, 2022
A dataset containing synchronized visual, inertial and GNSS raw measurements.

GVINS-Dataset Author/Maintainer: CAO Shaozu (shaozu.cao AT gmail.com), LU Xiuyuan (xluaj AT connect.ust.hk) This repository hosts dataset collected du

HKUST Aerial Robotics Group 134 Dec 21, 2022
Fuses IMU readings with a complementary filter to achieve accurate pitch and roll readings.

SimpleFusion A library that fuses accelerometer and gyroscope readings quickly and easily with a complementary filter. Overview This library combines

Sean Boerhout 5 Aug 22, 2022
📚🪛 Arduino library to calibrate and improve ADC measurements with the Raspberry Pi Pico.

Arduino-Pico-Analog-Correction Arduino library to calibrate and improve ADC measurements with the Raspberry Pi Pico. Can compensate ADC offsets, calcu

NuclearPhoenix 11 Jan 3, 2023
LIO-SAM源码详细注释,3D SLAM融合激光、IMU、GPS

LIO-SAM-DetailedNote LIO-SAM源码详细注释,3D SLAM融合激光、IMU、GPS,因子图优化。 LIO-SAM的代码十分轻量,只有四个cpp文件,很值得读一读呢。 关于LIO-SAM的论文解读,网上已经有很多文章啦,同系列的LOAM、A-LOAM、LEGO-LOAM等,在

Tao Lu 273 Dec 29, 2022
Unified Gaussian Preintegrated Measurements (UGPMs)

This repository provides the C++ implementation of the preintegration methods presented in our RSS'21 paper titled Continuous Integration over SO(3) for IMU Preintegration (with video here ). If you are using that code for any purpose, please cite the corresponding work as explained at the end of this page.

Centre for Autonomous Systems, University of Technology Sydney 77 Nov 30, 2022
Fix some extrinsic parameter importing problems. 6-axis IMU works now. Lidar without ring works now.

LVI-SAM-MODIFIED This repository is a modified version of LVI-SAM. Modification Add function to get extrinsic parameters.The original code assumes the

null 88 Dec 9, 2022
The MLX90614 is an Infra Red thermometer for noncontact temperature measurements.

The MLX90614 is an Infra Red thermometer for noncontact temperature measurements.

Shifeng Li 23 Dec 23, 2022