leggedrobotics / icp_localization Goto Github PK

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?

When I entered roslaunch icp_localization icp_node_rosbag.launch,the error occured, how can i solve it ? Thanks for your reply.

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.

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:

[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

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.`

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

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!

Bug Description

It often worked well but occasionally triggered:

malloc(): memory corruption

and then the [icp_node-2] process died:

[icp_node-2] process has died [pid 18948, exit code -6, cmd /home/admin/robotproject/icplocalization/catkin_ws/devel/lib/icp_localization/localizer_node __name:=icp_node __log:=/home/admin/.ros/log/92752c16-ada5-11ed-ad1e-c0b6f9cda807/icp_node-2.log].
log file: /home/admin/.ros/log/92752c16-ada5-11ed-ad1e-c0b6f9cda807/icp_node-2*.log 

The localization was perfect just until this bug occurred and it occurred randomly without patterns.

Detailed Information

... logging to /home/admin/.ros/log/92752c16-ada5-11ed-ad1e-c0b6f9cda807/roslaunch-admin-18912.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://admin:41469/

SUMMARY
========

PARAMETERS
 * /icp_node/calibration/imu_to_range_sensor/pitch: 0.0
 * /icp_node/calibration/imu_to_range_sensor/roll: 0.0
 * /icp_node/calibration/imu_to_range_sensor/x: 0.068
 * /icp_node/calibration/imu_to_range_sensor/y: 0.135
 * /icp_node/calibration/imu_to_range_sensor/yaw: 3.14159265359
 * /icp_node/calibration/imu_to_range_sensor/z: 0.275
 * /icp_node/calibration/odometry_source_to_range_sensor/pitch: 0.0
 * /icp_node/calibration/odometry_source_to_range_sensor/roll: 0.0
 * /icp_node/calibration/odometry_source_to_range_sensor/x: -0.076
 * /icp_node/calibration/odometry_source_to_range_sensor/y: -0.024
 * /icp_node/calibration/odometry_source_to_range_sensor/yaw: 0.0
 * /icp_node/calibration/odometry_source_to_range_sensor/z: 0.034
 * /icp_node/icp_config_path: /home/admin/wo...
 * /icp_node/icp_localization/fixed_frame: map
 * /icp_node/icp_localization/gravity_vector_filter_time_constant: 0.01
 * /icp_node/icp_localization/imu_data_topic: /imu/data
 * /icp_node/icp_localization/initial_pose/pitch: 0.0
 * /icp_node/icp_localization/initial_pose/roll: 0.0
 * /icp_node/icp_localization/initial_pose/x: 0.0
 * /icp_node/icp_localization/initial_pose/y: 0.0
 * /icp_node/icp_localization/initial_pose/yaw: 0.0
 * /icp_node/icp_localization/initial_pose/z: 0.0
 * /icp_node/icp_localization/is_provide_odom_frame: False
 * /icp_node/icp_localization/is_use_odometry: False
 * /icp_node/icp_localization/min_num_odom_msgs_before_ready: 5
 * /icp_node/icp_localization/num_accumulated_range_data: 1
 * /icp_node/icp_localization/odometry_data_topic: /camera/odom/sample
 * /icp_node/icp_localization/range_data_topic: /velodyne_points
 * /icp_node/input_filters_config_path: /home/admin/wo...
 * /icp_node/pcd_filename: /home/admin/wo...
 * /rosdistro: melodic
 * /rosversion: 1.14.13
 * /use_sim_time: True

NODES
  /
    icp_node (icp_localization/localizer_node)
    rosbag (rosbag/play)
    rviz_icp (rviz/rviz)

auto-starting new master
process[master]: started with pid [18934]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 92752c16-ada5-11ed-ad1e-c0b6f9cda807
process[rosout-1]: started with pid [18945]
started core service [/rosout]
process[icp_node-2]: started with pid [18948]
process[rosbag-3]: started with pid [18953]
process[rviz_icp-4]: started with pid [18954]
[ERROR] [1676516217.436628947]: 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/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/WrenchStamped
Map size is: 1316460
Init pose set to, xyz: 0 0 0, q: 0 0 0 1, rpy: 0 0 0 deg 
odometry data topic: /camera/odom/sample
imu data topic: /imu/data
Is use odometry: false
Gravity vector filter time constant: 0.01
Min num odom measurements before ready: 5
Calibration: 
imu to range sensor: t:[0.068000, 0.135000, 0.275000] ; q:[0.000000, 0.000000, 1.000000, 0.000000] ; rpy (deg):[0.000000, 0.000000, 180.000000]

odometry source to range sensor: t:[-0.076000, -0.024000, 0.034000] ; q:[0.000000, 0.000000, 0.000000, 1.000000] ; rpy (deg):[0.000000, 0.000000, 0.000000]

range data parameters: 
topic: /velodyne_points
num range data accumulated: 1
 
Setting fixed frame to: map
ICPlocalization: Initialized 
succesfully initialized icp
QObject::connect: Cannot queue arguments of type 'QVector<int>'
(Make sure 'QVector<int>' is registered using qRegisterMetaType().)
QObject::connect: Cannot queue arguments of type 'QVector<int>'
(Make sure 'QVector<int>' is registered using qRegisterMetaType().)
malloc(): memory corruption
[icp_node-2] process has died [pid 18948, exit code -6, cmd /home/admin/workspace/robotproject/icplocalization/catkin_ws/devel/lib/icp_localization/localizer_node __name:=icp_node __log:=/home/admin/.ros/log/92752c16-ada5-11ed-ad1e-c0b6f9cda807/icp_node-2.log].
log file: /home/admin/.ros/log/92752c16-ada5-11ed-ad1e-c0b6f9cda807/icp_node-2*.log
[rosbag-3] process has finished cleanly
log file: /home/admin/.ros/log/92752c16-ada5-11ed-ad1e-c0b6f9cda807/rosbag-3*.log

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.

Ｔhank 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
`

Why use the ethz-asl's libpointmatcher instead of leggedrobotics'?