{"id":19855218,"url":"https://github.com/leggedrobotics/icp_localization","last_synced_at":"2025-10-07T00:56:40.258Z","repository":{"id":47685249,"uuid":"375358252","full_name":"leggedrobotics/icp_localization","owner":"leggedrobotics","description":"This package provides localization in a pre-built map using ICP and odometry (or the IMU measurements).","archived":false,"fork":false,"pushed_at":"2023-01-18T08:07:31.000Z","size":15019,"stargazers_count":316,"open_issues_count":4,"forks_count":52,"subscribers_count":29,"default_branch":"master","last_synced_at":"2025-07-21T13:46:52.759Z","etag":null,"topics":["icp","lidar","localization","mapping","navigation","ros","slam"],"latest_commit_sha":null,"homepage":"https://rsl.ethz.ch","language":"C++","has_issues":true,"has_wiki":null,"has_pages":null,"mirror_url":null,"source_name":null,"license":"bsd-3-clause","status":null,"scm":"git","pull_requests_enabled":true,"icon_url":"https://github.com/leggedrobotics.png","metadata":{"files":{"readme":"README.md","changelog":null,"contributing":null,"funding":null,"license":"LICENSE","code_of_conduct":null,"threat_model":null,"audit":null,"citation":null,"codeowners":null,"security":null,"support":null}},"created_at":"2021-06-09T13:06:04.000Z","updated_at":"2025-07-15T20:35:15.000Z","dependencies_parsed_at":"2023-02-10T14:00:30.541Z","dependency_job_id":null,"html_url":"https://github.com/leggedrobotics/icp_localization","commit_stats":null,"previous_names":[],"tags_count":0,"template":false,"template_full_name":null,"purl":"pkg:github/leggedrobotics/icp_localization","repository_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/leggedrobotics%2Ficp_localization","tags_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/leggedrobotics%2Ficp_localization/tags","releases_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/leggedrobotics%2Ficp_localization/releases","manifests_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/leggedrobotics%2Ficp_localization/manifests","owner_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/owners/leggedrobotics","download_url":"https://codeload.github.com/leggedrobotics/icp_localization/tar.gz/refs/heads/master","sbom_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/leggedrobotics%2Ficp_localization/sbom","scorecard":null,"host":{"name":"GitHub","url":"https://github.com","kind":"github","repositories_count":278703574,"owners_count":26031205,"icon_url":"https://github.com/github.png","version":null,"created_at":"2022-05-30T11:31:42.601Z","updated_at":"2022-07-04T15:15:14.044Z","status":"online","status_checked_at":"2025-10-06T02:00:05.630Z","response_time":65,"last_error":null,"robots_txt_status":"success","robots_txt_updated_at":"2025-07-24T06:49:26.215Z","robots_txt_url":"https://github.com/robots.txt","online":true,"can_crawl_api":true,"host_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub","repositories_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories","repository_names_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repository_names","owners_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/owners"}},"keywords":["icp","lidar","localization","mapping","navigation","ros","slam"],"created_at":"2024-11-12T14:12:02.121Z","updated_at":"2025-10-07T00:56:40.193Z","avatar_url":"https://github.com/leggedrobotics.png","language":"C++","funding_links":[],"categories":[],"sub_categories":[],"readme":"# Localization using ICP in a known map\n\n## Overview\n\nThis 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.\n\nThe user should provide a reference map (point cloud) as as a `.pcd` file and an initial pose in the reference map.\n\nReleased under [BSD 3-Clause license](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.\n\n**Author:** Edo Jelavic\n\n**Maintainer:** Edo Jelavic, [jelavice@ethz.ch](jelavice@ethz.ch)\n\n| localization in a forest | localization in an urban environment |\n|:--------:|------------------|\n|[![forest](doc/localization_forest.gif)](doc/localization_forest.gif)|[![urban](doc/localization_urban.gif)](doc/localization_urban.gif)|\n\n\nThe package has been tested on the platforms shown in the images below. It has also been used during the ETH Robotic Summer School 2021 \u0026 2022\n ([link](https://robotics-summerschool.ethz.ch/)). The corresponding launch files can be found [here](https://github.com/ETHZ-RobotX/smb_common/tree/master/smb_slam).\n \nA corresponding map can for example be created using [Open3D SLAM](https://github.com/leggedrobotics/open3d_slam).\n \n| handheld sensor module | skid steer robot | legged robot |\n|:--------:|------------------|--------------|\n|[![handheld](doc/sensor_module.png)](doc/sensor_module.png)|[![skid-steer](doc/smb.png)](doc/smb.png)|[![anymal](doc/anymal.png)](doc/anymal.png)|\n\n\n\n\n## Installation\n\nClone the following three dependencies:\n```bash\n# in your source folder `src`\ngit clone https://github.com/leggedrobotics/libnabo.git\ngit clone https://github.com/leggedrobotics/libpointmatcher.git\ngit clone https://github.com/leggedrobotics/pointmatcher-ros.git\n```\n\nInstall ROS and library dependencies with:  \n```bash\nsudo 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\n# OR, use rosdep in your source folder `src` \nsudo rosdep install -yr --from-paths .\n```\n\nRecommended to build in release mode for performance (`catkin config -DCMAKE_BUILD_TYPE=Release`)\n\nBuild with:  \n```bash\ncatkin build icp_localization\n```\n\n## Usage\n\nThis package is based on the `libpointmatcher` package and it uses the ICP implementation from there. Libpointmatcher has an extensive [documentation](https://libpointmatcher.readthedocs.io/en/latest/). icp_localization provides ROS wrappers and uses either odometry or IMU measurements to calculate initial guesses for the pointcloud alignment.\n\nYou 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.\n\nYou can download the example bags and the example config files [here](https://drive.google.com/drive/folders/1XF3MUqT55m2beZYUe_IHQ4uhf6LF8m2J?usp=sharing). 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`.\n\nThe rosbag examples can be luaunched with `roslaunch icp_localization icp_node_rosbag.launch`\n\nNote 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.\n\nThe 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.\n\nThe 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.\n\nThe node publishes the TF tree: map-\u003eodom-\u003eodom_source-\u003erange_sensor (in case you are using the odometry).\n\n## Configuration\n\nThe configuration is split into three `.yaml` files.  \n\nThe `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.\n\nThe `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).   \n\nThe 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](https://libpointmatcher.readthedocs.io/en/latest/Configuration/#creating-custom-configurations-with-yaml).  \n\nThe rest of the parameters is explained below:\n\n* `icp_localization/initial_pose` - initial pose of the range sensor frame in the provided map.\n* `icp_localization/imu_data_topic` - ROS topic on which the imu data is published\n* `icp_localization/odometry_data_topic` - ROS topic on which the odometry data is published\n* `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.\n* `icp_localization/range_data_topic` - ROS topic on which the LIDAR data is published\n* `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.\n* `icp_localization/is_provide_odom_frame` - Whether to provide the odom frame or publish directly map to range sensor transformation only\n* `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.\n* `icp_localization/fixed_frame` - Fixed frame map. Used mostly for visualization.\n* `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.\n* `calibration` - calibration parameters between sensors. The coordinate frames are shown below. T\u003csub\u003eos_rs\u003c/sub\u003e is a transoformation from odometry source (e.g. tracking camera) to the range sensor frame.T\u003csub\u003eimu_rs\u003c/sub\u003e is the transformaion form the imu to the range sensor frame.\n\n\u003cimg src=\"doc/frames.png\" alt=\"frames\" width=\"345\" height=\"309\"/\u003e\n\nAll coordinate frames follow the URDF convention: http://wiki.ros.org/urdf/XML/joint.\n\n\n","project_url":"https://awesome.ecosyste.ms/api/v1/projects/github.com%2Fleggedrobotics%2Ficp_localization","html_url":"https://awesome.ecosyste.ms/projects/github.com%2Fleggedrobotics%2Ficp_localization","lists_url":"https://awesome.ecosyste.ms/api/v1/projects/github.com%2Fleggedrobotics%2Ficp_localization/lists"}