jyakaranda / ndt_map

SLAM package using NDT registration library of Autoware with loop-closure detection (odometry based) referenced from lego_loam.

NDT_MAP

Introduction

参考 Autoware 的 ndt_mapping 和 lego_loam 的回环检测实现的建图包。NDT 和 lego_loam 原理见博客。

loop-closure enabled map cloud loop-closure disabled map cloud loop-closure enabled trajectory loop-closure disabled trajectory

Dependency

• gtsam(Georgia Tech Smoothing and Mapping library, 4.0.0-alpha2)
• ndt_cpu
• ndt_gpu

Usage

Input

• Point Cloud(/lslidar_point_cloud)
• Odometry(/odom/imu)
• Imu(/imu/data)
• TF: /base_link -> /laser, /odom -> /base_link(/tf)

Output

• (/laser_cloud_surround)

Run the package

1. Run the launch file:

roslaunch ndt_map test.launch

2. Play existing bag files test_0515.bag:

rosbag play test_0515.bag --clock --topics

Issues

• 线程安全
• 优化点云配准的初始估计
• 增加非里程计的初始估计（目前在 use_odom 设为 false 的情况下有问题）
• pitch 的累计误差导致高度漂移问题
• 目前位姿有抖动情况，尤其是 z 轴，还未检查问题（与定位时的抖动比较像，应该可以一起解决）

TODOs

• 以均值-协方差格式存储/加载 NDT 地图
• 基于特征点的回环检测
• 添加 gpu 支持（在 ndt_gpu 库的基础上修改完成）
• 学习借鉴 Apollo NDT 定位的工程 trick

Reference

1. autowarefoundation/autoware
2. RobustFieldAutonomyLab/LeGO-LOAM
3. NDT 公式推导及源码解析（1）

ndt_map's Issues

Dependency

Hello,

How can I install dependencies??
ndt_cpu
ndt_gpu

Demo don't work well

Hello.
Thank you for a good job.
Your slam do not work well in your demo data in my environment.
How does this demo work?
Are the parameters okay in test.yaml ?
Please advice me.

• my environment
  ros melodic
  pcl 1.8

• mapping result
  

• terminal output

deen@deen-ubuntu:~/catkin_ws roslaunch ndt_map test.launch
... logging to /home/deen/.ros/log/9a8042b0-680c-11ea-8de0-a088690b5d10/roslaunch-deen-ubuntu-11559.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://deen-ubuntu:46745/

SUMMARY
========

PARAMETERS
 * /map_node/ds_history_size: 0.5
 * /map_node/history_fitness_score: 0.3
 * /map_node/history_search_num: 25
 * /map_node/history_search_radius: 15.0
 * /map_node/keyframe_dist: 0.3
 * /map_node/loop_closure_enabled: True
 * /map_node/max_iters: 30
 * /map_node/max_scan_range: 70
 * /map_node/min_scan_range: 1
 * /map_node/ndt_res: 1.0
 * /map_node/step_size: 0.15
 * /map_node/use_imu: True
 * /map_node/use_odom: True
 * /map_node/voxel_leaf_size: 1.0
 * /rosdistro: melodic
 * /rosversion: 1.14.3
 * /tf_b2l_pitch: 0.0
 * /tf_b2l_roll: 0.0
 * /tf_b2l_x: 0.0
 * /tf_b2l_y: 0.0
 * /tf_b2l_yaw: 0.0
 * /tf_b2l_z: 1.7
 * /use_sim_time: True

NODES
  /
    map_node (ndt_map/map_node)
    rviz (rviz/rviz)

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

setting /run_id to 9a8042b0-680c-11ea-8de0-a088690b5d10
process[rosout-1]: started with pid [11580]
started core service [/rosout]
process[rviz-2]: started with pid [11587]
process[map_node-3]: started with pid [11588]
[ WARN] [1584421428.830847990, 1557902277.384249395]: unsync imu and pc msg
[ WARN] [1584421428.830902623, 1557902277.384249395]: unsync imu and pc msg
[ WARN] [1584421428.830925847, 1557902277.384249395]: unsync imu and pc msg
[ WARN] [1584421428.830937199, 1557902277.384249395]: unsync imu and pc msg
[ WARN] [1584421428.830955647, 1557902277.384249395]: unsync imu and pc msg
……

Odometry(/odom/imu)

里程计是使用什么？
编码器的还是IMU进行积分获得的？