mcl_3dl

Package summary

mcl_3dl is a ROS node to perform a probabilistic 3-D/6-DOF localization system for mobile robots with 3-D LIDAR(s). It implements pointcloud based Monte Carlo localization that uses a reference pointcloud as a map.

The node receives the reference pointcloud as an environment map and localizes 6-DOF (x, y, z, yaw, pitch, roll) pose of measured pointclouds assisted by a motion prediction using odometry.

Currently, the supported motion model is differential-wheeled-robot. The node provides classic MCL; currently, it doesn’t implement adaptive feature like KDL-sampling and etc.

Algorithms

A fundamental algorithm of mcl_3dl node is Monte Carlo localization (MCL), aka particle filter localization. MCL represents a probabilistic distribution of estimated pose as density and weight of particles and estimates the pose from the distribution.

• Algorithm details
• Parameters

Node I/O

Install

from source

Note: mcl_3dl_msgs package is required to build mcl_3dl package.

# clone
cd /path/to/your/catkin_ws/src
git clone https://github.com/at-wat/mcl_3dl.git
git clone https://github.com/at-wat/mcl_3dl_msgs.git

# build
cd /path/to/your/catkin_ws
rosdep install --from-paths src --ignore-src -y  # Install dependencies
catkin_make -DCMAKE_BUILD_TYPE=Release  # Release build is recommended

from apt repository (for ROS Indigo/Kinetic/Lunar on Ubuntu)

sudo apt-get install ros-${ROS_DISTRO}-mcl-3dl

Running the demo

The example bag file of 2+4-DOF tracked vehicle with two Hokuyo YVT-X002 3-D LIDAR is available online. Pre-processed (filtered) 3-D pointcloud, IMU pose, odometry, and map data are packed in the bag.

# Download the example bag (230M)
wget -P ~/Downloads https://openspur.org/~atsushi.w/dataset/mcl_3dl/short_test3.bag

# Running the demo
roslaunch mcl_3dl test.launch use_pointcloud_map:=false use_cad_map:=false \
  use_bag_file:=true bag_file:=${HOME}/Downloads/short_test3.bag

The map data in the bag was generated by using the cartographer_ros and filtered by using pcl_outlier_removal and pcl_voxel_grid utilities.

MarkerArray shows several mcl_3dl internal information.

• Purple spheres: sampled points used in the likelihood-model calculation
• Red lines: casted rays in the beam-model calculation
• Red boxes: detected collisions in raycasting

To try global localization, call /global_localization by the following command.

rosservice call /global_localization

Demos without odometry and without IMU are also available.

Contributing

mcl_3dl package is developed under GitHub flow. Feel free to open new Issue and/or Pull Request.

The code in this repository is following ROS C++ Style Guide. A configuration file for clang-format is available at https://github.com/seqsense/ros_style/.

License

• mcl_3dl is provided under the BSD license.
• Backport codes of Point Cloud Library (PCL) is provided under the BSD license.

Changelog for package mcl_3dl

0.6.4 (2024-10-10)

• Fix angular part of covariance matrix (#417)
• Contributors: f-fl0

0.6.3 (2024-07-22)

• Explicitly handle zero sigma (#414)
• Update assets to v0.6.4 (#413)
• Update assets to v0.6.3 (#412)
• Update assets to v0.6.2 (#411)
• Contributors: Atsushi Watanabe

0.6.2 (2023-11-14)

• Fix reported entropy (#408)
• Update assets to v0.6.1 (#407)
• Update assets to v0.6.0 (#406)
• Update assets to v0.5.2 (#405)
• Update assets to v0.5.1 (#404)
• Update assets to v0.4.2 (#402)
• Contributors: Atsushi Watanabe, f-fl0

0.6.1 (2023-01-04)

• Update assets to v0.4.1 (#400)
• Support PCL 1.11 and later (#397)
• Update assets to v0.4.0 (#395)
• Update assets to v0.3.4 (#392)
• Remove old workarounds for PCL<1.8 (#389)
• Update assets to v0.3.3 (#388)
• Update assets to v0.3.2 (#387)
• Update code format (#386)
• Contributors: Atsushi Watanabe

0.6.0 (2021-05-12)

• Add option to load cloud through "load_pcd" service (#381)
• Update assets to v0.3.1 (#382)
• Update assets to v0.3.0 (#380)
• Update assets to v0.2.0 (#379)
• Contributors: Atsushi Watanabe, Remco

0.5.4 (2021-03-07)

• Add odom/imu/cloud_queue_size params (#375)
• Fix flaky tests (#376)
• Contributors: Atsushi Watanabe

0.5.3 (2021-02-26)

• Fix cloud accum reset on map update (#371)
• Contributors: Atsushi Watanabe

0.5.2 (2021-01-15)

• Fix potential "Time is out of dual 32-bit range" error (#367)
• Update assets to v0.1.4 (#365)
• Improve test stability (#363)
• Update assets to v0.1.3 (#362)
• Update assets to v0.1.2 (#361)
• Migrate to GitHub Actions (#357)
• Update assets to v0.0.10 (#356)
• Contributors: Atsushi Watanabe

0.5.1 (2020-10-26)

• Make hit_range independent from grid size and fix DDA hit/miss state (#350)
• Fix crushing when lidar poses are out of map (#351)
• Contributors: Atsushi Watanabe, Naotaka Hatao

0.5.0 (2020-10-15)

• Fix crushing when new map is received (#347)
• Ease condition for test of PointCloudSamplerWithNormal (#344)
• Add faster raycast algorithm using DDA (#343)
• Contributors: Naotaka Hatao

0.4.0 (2020-10-07)

• Fix typos of license (#340)
• Add PointCloudSamplerWithNormal (#339)
• Contributors: Naotaka Hatao

0.3.0 (2020-09-07)

• Switch beam model by map label field (#334)
• Update test script for latest catkin (#333)
• Remove references to sensor_msgs::PointCloud (#332)
• Update assets to v0.0.9 (#331)
• Improve expansion resetting/global localization test stability (#330)
• Fix global localization test parameter (#328)
• Avoid rate limit when fetching gh-ph-comment (#329)
• Update gh-pr-comment (#327)
• Retry codecov script download (#326)
• Improve test coverage (#325)
• Merge rostest coverage profiles (#324)
• Contributors: Atsushi Watanabe, f-fl0

0.2.5 (2020-05-27)

• Add validation for orientation of initial pose (#317)
• Update CI scripts (#318)
• Contributors: Atsushi Watanabe, Yuta Koga

0.2.4 (2020-05-08)

• Fix resampling failure of last particle (#313)
• Retry gpg keyserver on prerelease test (#312)
• Add filter class for Vec3 (#311)
• Refactor math functions (#310)
• Fix deprecation warning (#309)
• Split parameter loader code (#307)
• Contributors: Atsushi Watanabe, Naotaka Hatao

0.2.3 (2020-04-07)

• Update assets to v0.0.8 (#303)
• Fix flaky rostest nodes (#302)
• Update E2E test parameters (#301)
• Refactor CI scripts (#300)
• Add Noetic CI job (#296)
• Fix initialization of accumulated cloud header (#299)
• Support Noetic (#297)
• Contributors: Atsushi Watanabe

0.2.2 (2020-03-30)

• Make average number of accumulated clouds accurate (#293)
• Fix latching flag in demo bag (#294)
• Fix cloud accumulation logic (#290)
• Contributors: Atsushi Watanabe

0.2.1 (2020-02-03)

• Set DiagnosticStatus::OK as default (#283)
• Update assets to v0.0.7 (#282)
• Contributors: Atsushi Watanabe, Daiki Maekawa

0.2.0 (2020-01-18)

• Install consistent version of ros_buildfarm (#281)
• Run prerelease test with latest msgs package (#278)
• Expose internal errors and convergence status (#265)
• Document motion prediction model parameters (#277)
• Contributors: Atsushi Watanabe, Daiki Maekawa

0.1.7 (2020-01-06)

• Update assets to v0.0.6 (#273)
• Update assets to v0.0.5 (#272)
• Add catkin/bloom release actions (#269)
• Fix codecov setting (#270)
• Fix codecov config (#268)
• Migrate C math functions to C++ (#267)
• Enable particle initialization using covariances (#259)
• Contributors: Atsushi Watanabe, Naotaka Hatao

0.1.6 (2019-10-21)

• Clear odometry integration error on global localization (#257)
• Accelerate CI and prerelease test (#254)
• Contributors: Atsushi Watanabe

0.1.5 (2019-08-15)

• Split prerelease test job to avoid CI time-limit (#251)
• Speed-up covariance calculation during global localization (#248)
• Run prerelease test on release preparation PR (#246)
• Add a test for landmark measurement (#242)
• Add test case for expansion resetting service (#241)
• Test matched/unmatched debug outputs (#240)
• Add basic tests for State6DOF class (#239)
• Fix demo configuration and update README (#238)
• Add no-imu and no-odometry modes (#234)
• Add test case for obsolated compatibility mode (#237)
• Fix document file permission (#236)
• Add test for compatibility level check (#235)
• Fix ParticleFilter::resample() (#233)
• Drop ROS Indigo and Ubuntu Trusty support (#230)
• Disable CI build for indigo (#229)
• Refactor motion prediction models (#227)
• Refactor IMU measurement models (#226)
• Fix include directory priority (#225)
• Custom point type (#206)
• Fix transform object constness (#224)
• Add LICENSE file (#220)
• Add post-release test script (#218)
• Reduce memcpy in point cloud transform (#216)
• Contributors: Atsushi Watanabe, Naotaka Hatao

0.1.4 (2018-12-20)

• Fix IO figure (#212)
• Fix tf timestamp (#214)
• Add pf::ParticleFilter::appendParticle (#207)
• Fix pointer alignment style (#210)
• Migrate tf to tf2 (#208)
• Fix class member naming style (#205)
• Make lidar measurement model class (#195)
• Add I/O diagram to the document (#199)
• Update Algorithms.md (#198)
• Add apt-get upgrade to test Dockerfiles (#197)
• Add document for expansion resetting (#193)
• Add test for expansion resetting (#192)
• Add test for global localization (#188)
• Refactor likelihood calculation (#189)
• Add a comment to test_transform_failure (#184)
• Build mcl_3dl_msgs from source on CI (#185)
• Fix resampling (#183)
• Fix test failure on ROS buildfarm (#181)
• Fix catkin package definitions (#180)
• Add tf exception handling and change message level (#177)
• Relax codecov patch threshold (#179)
• Allow small coverage drop (#178)
• Fix test names (#176)
• Add build id to CI bot comment (#174)
• Fold CI bot comment (#173)
• Decrease bag playback rate in integration test (#172)
• Add test for NormalLikelihoodNd (#171)
• Report coverage only after successful test (#170)
• Add CI badges (#169)
• Add codecov covarage test (#168)
• Fix bot comment target slug (#167)
• Contributors: Atsushi Watanabe, So Jomura

0.1.3 (2018-06-23)

• Fix install of demo launch and config (#164)
• Update CI and add test on ROS Melodic (#155)
• Ignore gh-pr-comment failure (#162)
• Compile with PCL_NO_PRECOMPILE (#161)
• Fix rostest dependency (#160)
• Fix roslint dependency (#159)
• Update install instructions in README (#158)
• Update manifest format and fix CMakeLists (#157)
• Use mcl_3dl_msgs package (#152)
• Test with shadow-fixed repository (#154)
• Update CI bot environments (#150)
• Add encrypted token for image caching (#149)
• Fix migration instruction message (#147)
• Fix match ratio min/max check (#146)
• Add interfaces to ChunkedKdtree for external usages (#145)
• Install headers (#143)
• Contributors: Atsushi Watanabe

0.1.2 (2018-04-27)

• Workaround for debian stretch build (#140)
• Contributors: Atsushi Watanabe

0.1.1 (2018-04-25)

• Update CI settings (#136)
• Remove CMake warning message (#134)
• Contributors: Atsushi Watanabe

0.1.0 (2018-04-23)

• Migrate to ROS recommended namespace model (#130)
• Minor CI setting updates (#129)
• Fix package deps (#127)
• Fix dockerfile style (#125)
• Load CI cache from docker hub registry (#124)
  • also add build matrix
• Add raycast performance benchmark (#123)
• Fix GLOBAL_LOCALIZATION status (#122)
• Add localization status output (#120)
• Fix nodehandle usage (#121)
• Update demo without odometry (#119)
  • Update demo without odometry
  • Update README
  • Add document of the demo without odometry
• Move sample parameters in launch into yamls (#72)
• Fix time jump back (#117)
  • Fix time jump back
  • Add warning of time jump
  • Fix tf error check
• Add unit tests for Raycast (#116)
  • Add unit tests for Raycast
  • Fix raycast grid handling
• Chunked kd-tree (#113)
  • Add chunked kd-tree to remove map truncation
  • Remove unused params
  • Remove unused debug output
  • Add unit test for ChunkedKdtree
• Update test reference checksum (#114)
• Fix raycast collision tolerance (#112)
  • Tolerance of the end of the raycast was too small in 1a758c0 because of the increase of the search range.
• Add integral angular odometry error constraint (#111)
• Fix raycast (#110)
  • Hit was checked by using range search with (grid/2.0) which make a lot of miss detection. (sqrt(2.0) * grid / 2.0) should be good approximation.
• Add rule based expansion resetting (#109)
• Fix integral odom error debug output (#108)
• Add landmark measurement input (#107)
• Fix map update timer (#105)
• Fix CI bot (#104)
  • Fix repository url
  • Use pip version of the bot
• Remove spinOnce polling and waitForTransform (#102)
  • Use ros::Timer instead of ros::spinOnce polling
  • Remove waitForTransform for static transforms
  • Remove waitForTransform for buffered (delayed) objects
• Fix particle initialization (#101)
• Reset integral odometry error if jumped (#100)
• Add constraint on the integral of odometry error (#99)
  • odom_err_integ_tc: time constant to hold the integral of the odometry error
  • odom_err_integ_sigma: acceptable range of the integral of the odometry error
• Visualize sampled points and raycasting result (#97)
  • Visualize sampled points and raycasting result
  • Remove duplicated code around raycasting
• Fix raycasting accuracy (#96)
• Fix odometry noise function in prediction (#95)
• Add global localization (#91)
• Fix particle resize (#92)
  • same fix as #90
• Fix resampling for huge particle size (#90)
  • All-zero particles have appeared on resampling if the particle size is very large.
  • Also, add iterator.
• Add test for pf::ParticleFilter. (#89)
• Build test with -Wall -Werror. (#88)
  • Build test with -Wall -Werror.
  • Workaround for invalid macro name bug in PCL(<1.8.1).
• Fix odometry noise function. (#87)
  • wrong: [nd(mean = 1.0, sigma = sigma_trans_trans) * nd(mean = 1.0, sigma = sigma_rot_trans)]{.title-ref}
  • corrected: [nd(mean = 0.0, sigma = sigma_trans_trans) + nd(mean = 0.0, sigma = sigma_rot_trans)]{.title-ref}
• Skip random points sampling if all points are filtered out. (#86)
• Fix build on indigo. (#84)
• Add map_clip_far param. (#85)
• Support variable particle size. (#78)
  • Support variable particle size.
  • Add service to change particle size.
  • Add test for resizeParticle.
• Check input cloud size. (#82)
  • Check for empty cloud to avoid failure on kdtree build.
  • Fix usage of point size of pcl::PointCloud.
• Remove debug outputs. (#81)
• Use online version of test result comment bot. (#80)
• Fix const function attributes. (#77)
• Remove dummy dep to system_lib. (#76)
• Add unit tests for mathematical classes. (#74)
  • Add unit tests for Vec3, Quat, NormalLikelihood, Filter classes.
  • Fix scaling of the NormalLikelihood distribution.
  • Fix Filter::set in angle mode.
• Fix naming styles. (#73)
  • Names of the classes and their members now get compatible with ROS recommended coding styles.
  • Public member variables are kept without underscore postfix.
• Fix package install. (#71)
• Fix assert of sampled point amount check. (#70)
• Fix quaternion average and use expectation as estimation result. (#67)
• Fix bot's test result posting on fail. (#68)
• Include test result on bot post. (#66)
• Fix a bug where all particle probabilities get zero. (#65)
  • fix number of selected points for likelihood calculation
  • add error recovering / asserts
• fixes coding styles (#64)
• adds parameter to accumulate input clouds (#60)
• syncs tf timestamp with last odometry (#61)
• adds example without odometry (#57)
• updates default params and demo (#55)
• adds option to disable tf publish and test for tf output (#46)
• adds test result notifier bot (#53)
• fixes possibly invalid memory access (#52)
• changes docker storage driver to overlay2 (#51)
• adds pcd file output of all pointcloud (#50)
• limits minimum beam_model likelihood (#49)
• separates point ranges of beam model and fixes total ref reduction (#48)
• makes acc measurement variance configurable (#47)
• fixes published tf timestamps to have a future date (#45)
• fixes docker caching on travis (#43)
• updates default parameters (#42)
• adds debug visualization output of casted ray (#41)
• fixes total reflection reduction (#40)
• rejects total reflection points in beam_model (#37)
• fixes test result handling and playback rate (#38)
• ignores travis run on non-master branch (#36)
• caches test dataset outside of docker (#34)
  • caches test dataset outside docker
  • changes script path
• adds travis settings for a test in docker container (#33)
• adds localization accuracy test (#32)
• makes beam_model likelihood configurable (#30)
• removes ad-hoc map filter (#27)
• updates sample launch file (#28)
  • The commit enables:
    • IMU measurement
    • loading map from pcd file
• adds imu measurement (#26)
• adds hysteresis on final estimation (#24)
• updates parameters in sample launch file (#23)
  • removes map offset parameters
  • specifies jump detection distance
• fixes axis-angle value range (#22)
• updates parameters in sample launch file (#19)
• fixes odometry error parameter handling (#18)
• fixes beam_model raycast origin (#17)
• adds parameter to specify odometry error
• adds sample launch file (#14)
  • This fixes #3.
  • A dataset for testing will be supplied in future.
• adds documentation (#10)
• fixes init_yaw/pitch/roll setting (#12)
• ad hoc fix to a bug on PCL-1.7 with C++11
  • fixes #9
• adds matched/unmatched pointclouds output (#7)
• fixes filter resetting in angular mode
  • This commit fixes #2.
• makes map clipping parameters configurable
• fixes roll and pitch motion in prediction phase
• adds /amcl_pose output
  • This commit fixes #1.
• applies LPF on debugging output pointcloud coordinate
• changes default map frame to 'map' instead of 'map_ground'
• outsources map update
• adds beam model
• makes z clipping parameters configurable
• adds parameter to skip measurement
• reduces almost invisible points in map
• checks localization covariance on map update
• detects pose jump and reset LPF
• makes some parameters configurable
• adds covariance calculation
• uses rpy variance instead of quat
• supports jump back
• fixes PointRepresentation dimension
• speed up by using radiusSearch instead of nearestKSearch
• improves prediction phase
• adds flexible particle operators
• removes garbage semicolons
• makes matching related parameters configurable
• makes several parameters configurable
• adds output filter
• adds weight in matching
• adds some parameters
• reduces number of points of updated map cloud
• adds particleBase::operator+
• clips and updates maps
• adds vec3::operator*
• adds arg to specify sigma to resampling
• avoids memory access error in max()
• supports tf and initialpose
• supports quat::inverse
• supports vec3::operator-
• updates test parameters
• update map cloud
• accumulates clouds
• fixes resampling
• first test version
• Contributors: Atsushi Watanabe