---
# System prepended metadata

title: 3D mapping
tags: [localization, SLAM]

---

# 3D mapping
1. hdl_graph_slam https://github.com/koide3/hdl_graph_slam

2. cartographer https://google-cartographer-ros.readthedocs.io/en/latest/compilation.html
https://blog.csdn.net/weixin_44521188/article/details/117812875

3. strands_3d_mapping https://github.com/strands-project/strands_3d_mapping

5. LIO-SAM https://github.com/TixiaoShan/LIO-SAM
>该方法使用多传感器融合的方法，利用因子图优化，计算位姿；
可以把节点（位姿）理解成一个待求解的变量；各种传感器数据构建的约束当成一个方程组； 通过不断加入各种因子，相当于给方程组中加入更多的方程，联合来求解最小二乘问题。
可以看出，回环检测部分基本和Lego-LOAM的特点一致，即不具备重定位能力（该方法前提是需要知道自己的大致位置，和历史中附近的位置进行匹配

* CMakeLists.txt
    ```
    find_package(Boost REQUIRED timer serialization thread chrono)
    set(PCL_DIR "/usr/local/share/pcl-1.9/PCLConfig.cmake")
    set(CMAKE_CXX_FLAGS "-std=c++14")
    ```
    
* utility.h
    ```
    #include <opencv2/opencv.hpp>
    ```
    
* Run the launch file:
    ```
    $ roslaunch lio_sam run.launch

    ... logging to /home/yufan/.ros/log/b5f78ae2-9c5f-11ec-a68f-cde8fd81d13b/roslaunch-yufan-90780.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.

    xacro: in-order processing became default in ROS Melodic. You can drop the option.
    started roslaunch server http://yufan:41655/

    SUMMARY
    ========

    PARAMETERS
     * /ekf_gps/base_link_frame: base_link
     * /ekf_gps/frequency: 50
     * /ekf_gps/imu0: imu_correct
     * /ekf_gps/imu0_config: [False, False, Fa...
     * /ekf_gps/imu0_differential: False
     * /ekf_gps/imu0_queue_size: 50
     * /ekf_gps/imu0_remove_gravitational_acceleration: True
     * /ekf_gps/map_frame: map
     * /ekf_gps/odom0: odometry/gps
     * /ekf_gps/odom0_config: [True, True, True...
     * /ekf_gps/odom0_differential: False
     * /ekf_gps/odom0_queue_size: 10
     * /ekf_gps/odom_frame: odom
     * /ekf_gps/process_noise_covariance: [1.0, 0, 0, 0, 0,...
     * /ekf_gps/publish_tf: False
     * /ekf_gps/sensor_timeout: 0.01
     * /ekf_gps/two_d_mode: False
     * /ekf_gps/world_frame: odom
     * /lio_sam/Horizon_SCAN: 1800
     * /lio_sam/N_SCAN: 16
     * /lio_sam/baselinkFrame: base_link
     * /lio_sam/downsampleRate: 1
     * /lio_sam/edgeFeatureMinValidNum: 10
     * /lio_sam/edgeThreshold: 1.0
     * /lio_sam/extrinsicRPY: [1, 0, 0, 0, 1, 0...
     * /lio_sam/extrinsicRot: [1, 0, 0, 0, 1, 0...
     * /lio_sam/extrinsicTrans: [0.0, 0.0, 0.0]
     * /lio_sam/globalMapVisualizationLeafSize: 1.0
     * /lio_sam/globalMapVisualizationPoseDensity: 10.0
     * /lio_sam/globalMapVisualizationSearchRadius: 1000.0
     * /lio_sam/gpsCovThreshold: 2.0
     * /lio_sam/gpsTopic: odometry/gpsz
     * /lio_sam/historyKeyframeFitnessScore: 0.3
     * /lio_sam/historyKeyframeSearchNum: 25
     * /lio_sam/historyKeyframeSearchRadius: 15.0
     * /lio_sam/historyKeyframeSearchTimeDiff: 30.0
     * /lio_sam/imuAccBiasN: 0.000559833754009...
     * /lio_sam/imuAccNoise: 0.012601034335777903
     * /lio_sam/imuGravity: 9.80511
     * /lio_sam/imuGyrBiasN: 2.697272405810161...
     * /lio_sam/imuGyrNoise: 0.001614596902334...
     * /lio_sam/imuRPYWeight: 0.01
     * /lio_sam/imuTopic: IMU_data
     * /lio_sam/lidarFrame: velodyne
     * /lio_sam/lidarMaxRange: 1000.0
     * /lio_sam/lidarMinRange: 1.0
     * /lio_sam/loopClosureEnableFlag: True
     * /lio_sam/loopClosureFrequency: 1.0
     * /lio_sam/mapFrame: map
     * /lio_sam/mappingCornerLeafSize: 0.1
     * /lio_sam/mappingProcessInterval: 0.15
     * /lio_sam/mappingSurfLeafSize: 0.2
     * /lio_sam/numberOfCores: 4
     * /lio_sam/odomTopic: odom
     * /lio_sam/odometryFrame: odom
     * /lio_sam/odometrySurfLeafSize: 0.2
     * /lio_sam/pointCloudTopic: velodyne_points
     * /lio_sam/poseCovThreshold: 25.0
     * /lio_sam/rotation_tollerance: 6.28
     * /lio_sam/savePCD: True
     * /lio_sam/savePCDDirectory: /Downloads/LOAM/
     * /lio_sam/sensor: velodyne
     * /lio_sam/surfFeatureMinValidNum: 100
     * /lio_sam/surfThreshold: 0.1
     * /lio_sam/surroundingKeyframeDensity: 2.0
     * /lio_sam/surroundingKeyframeSearchRadius: 50.0
     * /lio_sam/surroundingKeyframeSize: 50
     * /lio_sam/surroundingkeyframeAddingAngleThreshold: 0.2
     * /lio_sam/surroundingkeyframeAddingDistThreshold: 1.0
     * /lio_sam/useGpsElevation: False
     * /lio_sam/useImuHeadingInitialization: False
     * /lio_sam/z_tollerance: 1
     * /navsat/broadcast_utm_transform: False
     * /navsat/broadcast_utm_transform_as_parent_frame: False
     * /navsat/delay: 0.0
     * /navsat/frequency: 50
     * /navsat/magnetic_declination_radians: 0
     * /navsat/publish_filtered_gps: False
     * /navsat/wait_for_datum: False
     * /navsat/yaw_offset: 0
     * /navsat/zero_altitude: True
     * /robot_description: <?xml version="1....
     * /rosdistro: noetic
     * /rosversion: 1.15.14

    NODES
      /
        ekf_gps (robot_localization/ekf_localization_node)
        lio_sam_featureExtraction (lio_sam/lio_sam_featureExtraction)
        lio_sam_imageProjection (lio_sam/lio_sam_imageProjection)
        lio_sam_imuPreintegration (lio_sam/lio_sam_imuPreintegration)
        lio_sam_mapOptmization (lio_sam/lio_sam_mapOptmization)
        lio_sam_rviz (rviz/rviz)
        navsat (robot_localization/navsat_transform_node)
        robot_state_publisher (robot_state_publisher/robot_state_publisher)

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

    setting /run_id to b5f78ae2-9c5f-11ec-a68f-cde8fd81d13b
    process[rosout-1]: started with pid [90800]
    started core service [/rosout]
    process[lio_sam_imuPreintegration-2]: started with pid [90807]
    process[lio_sam_imageProjection-3]: started with pid [90808]
    process[lio_sam_featureExtraction-4]: started with pid [90809]
    process[lio_sam_mapOptmization-5]: started with pid [90810]
    process[robot_state_publisher-6]: started with pid [90816]
    process[lio_sam_rviz-9]: started with pid [90826]
    [ INFO] [1646469543.762151904]: ----> Image Projection Started.
    [ INFO] [1646469543.769490270]: ----> Feature Extraction Started.
    [ INFO] [1646469543.788025292]: ----> Map Optimization Started.
    [ INFO] [1646469543.974031587]: ----> IMU Preintegration Started.    
    ```
    

* Play existing bag files:
    ```
    $ rosbag play bagname.bag -r 3
    ```


* save map as a PCD file.
    ```
    $ rosservice call /lio_sam/save_map 0.2 "/Downloads/LOAM/"
    success: True
    ```
    
    in roslaunch lio_sam run.launch terminal output
    ```
    ****************************************************
    Saving map to pcd files ...
    Save destination: /home/yufan/home/yufan/Desktop/LOAM/
    Processing feature cloud 82 of 83 ...

    Save resolution: 0.2
    ****************************************************
    Saving map to pcd files completed
    ```
* pcd to pgm
    ```
    $ rosrun pcd2pgm pcd2topic
    [ INFO] [1646470710.603409214]: *** file_directory = /home/yufan/Downloads/LOAM/ ***

    [ INFO] [1646470710.604926565]: *** file_name = GlobalMap ***

    [ INFO] [1646470710.604959980]: *** pcd_file = /home/yufan/Downloads/LOAM/GlobalMap.pcd ***

    初始点云数据点数：142778
    直通滤波后点云数据点数：100742
    [ INFO] [1646470710.622116916]: data size = 212380
    
    $ rosrun map_server map_saver -f map
    [ INFO] [1646470798.446913612]: Waiting for the map
    [ INFO] [1646470799.603431089]: Received a 410 X 518 map @ 0.050 m/pix
    [ INFO] [1646470799.603469728]: Writing map occupancy data to map.pgm
    [ INFO] [1646470799.606415156]: Writing map occupancy data to map.yaml
    [ INFO] [1646470799.611189432]: Done 
    ```
5. LeGO-LOAM https://github.com/RobustFieldAutonomyLab/LeGO-LOAM
>没有回环检测（前述已经说过） ；

> 计算时间复杂度较高，基于三维空间中的位姿进行优化；

>户外可能受到各种噪声影响，例如树上摇晃的树叶，地上的杂草。而这些点未必会重复出现在前后两帧激光中。而错误的特征点将会影响位姿精度。

>LOAM需要提取平面点和边缘点，由于车体上下颠簸，竖直维度提取的平面点很容易造成误差。


* 保存地图
```
#結束前須先開啟
rosbag record /laser_cloud_surround

完成後在執行
rosrun pcl_ros bag_to_pcd name.bag /laser_cloud_surround pcd1
cd /pcd1
pcl_viewer name.pcd
```
6. loam_velodyne https://github.com/laboshinl/loam_velodyne

7. A-LOAM https://github.com/HKUST-Aerial-Robotics/A-LOAM


| method | carto | lio-sam | lego-loam | hdl_graph |
| -------- | -------- | -------- | -------- | -------- |
| based | Graph     | Features     | Features | Graph |
| dis error(m) | 14.8~15.14     | 14.7~14.9     | y roation | 14.9 |
| pointcloud(points)     | many(35536612)   | normal(100219) | low(6126) | normal(115428) |
| recommend     |  3  |   2   | 4 | 1 |

* lego-loam 點雲稀疏且輸出pgm有問題無法2d導航使用
* carto 點雲數量龐大且會有綁架問題
* lio-sam 測試工廠環境需要資料集測試
* hdl-graph目前建圖狀況較穩定適合辦公室情境,如須測試工廠環境需要資料集測試
# 3D localization
###### tags:`SLAM` `localization`

1. **amcl3d** https://github.com/fada-catec/amcl3d
2. **cartographer** https://google-cartographer-ros.readthedocs.io/en/latest/compilation.html
3. **velodyne_points** https://github.com/koide3/hdl_localization

4. **mcl_3dl** https://github.com/at-wat/mcl_3dl
    ### install 
    ```
    $　sudo apt-get install ros-${ROS_DISTRO}-mcl-3dl
    ```
    ### 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
    ```
    
4. **DLL** https://github.com/robotics-upo/dll
    ### install 
    * [pcd to bt](https://github.com/gaoxiang12/octomap_tutor/blob/master/src/pcd2octomap.cpp)
    ```
    $ sudo apt-get install libopencv-de libboost-all-dev
    $ wget https://github.com/OctoMap/octomap/archive/refs/tags/v1.9.6.tar.gz
    $ cd octomap
    $ mkdir build
    $ cd build
    $ cmake ..
    $ make

    #如果编译没出错，即完成安装，完成之后可以运行如下代码查看例子
    $ cd .. 
    $ bin/octovis octomap/share/data/geb079.bt
    ```
    如下效果：
    ![](https://i.imgur.com/DPA3M0f.png)

    * octomap_tutor/cmake_modules/octomap-config.cmake
    ```
    set(OCTOMAP_INCLUDE_DIRS "/home/yang/octomap/octomap/include")
    set(OCTOMAP_LIBRARY_DIRS "/home/yang/octomap/lib")


    # Set library names as absolute paths:
    set(OCTOMAP_LIBRARIES
      "/home/yang/octomap/lib/liboctomap.so"
      "/home/yang/octomap/lib/liboctomath.so"

    $ ./build.sh   
    ```
    * octomap_tutor/CMakeLists.txt
    ```
    SET(CMAKE_CXX_FLAGS -std=c++14)
    ```

    * demo
    ```
    $ cd octomap_tutor
    $ bin/pcd2octomap data_path_input/sample.pcd data_path_output/sample.bt
    point cloud loaded, piont size = 204186
    copy data into octomap...
    done.
    
    $ sudo apt install octovis
    $ pcl_viewer data/sample.pcd 須先開啟pcd在開啟bt
    $ octovis data/sample.bt 
    ```
    如下效果：
    ![](https://i.imgur.com/zh2T3p6.png)
    
    ### demo
    * 修改airsim1.launch(real time)
    ```
    <param name="use_sim_time" value="false" /> 
    <node pkg="tf" type="static_transform_publisher" name="odom_tf" args="0 0 0 0 0 0 odom base_Link 10" />
    <arg name="base_frame_id" default="base_link"/>
    <arg name="initial_x" default="0.0"/> 
    <arg name="initial_y" default="0.0"/> 
    <arg name="initial_z" default="0.0"/> 
    <arg name="initial_a" default="0.0"/> 
    <arg name="map" default="test.bt" />
    <arg name="map_path" default="/home/kenmec/catkin_ws/src/LIO-SAM/$(arg map)"/>
    <remap from="/imu" to="/IMU_data"/>
    <param name="in_cloud" value="/velodyne_points" />
    ```
    
    ```
    $ roslaunch dll airsim1.launch
    ... logging to /home/kenmec/.ros/log/d38f1aec-9e95-11ec-a0ea-cd8b567011ed/roslaunch-kenmec-22767.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://kenmec:41131/

    SUMMARY
    ========

    PARAMETERS
     * /dll_node/align_method: 1
     * /dll_node/base_frame_id: base_link
     * /dll_node/global_frame_id: map
     * /dll_node/in_cloud: /velodyne_points
     * /dll_node/initial_a: 0.0
     * /dll_node/initial_x: 0.0
     * /dll_node/initial_y: 0.0
     * /dll_node/initial_z: 0.0
     * /dll_node/map_path: /home/kenmec/catk...
     * /dll_node/odom_frame_id: odom
     * /dll_node/publish_point_cloud: True
     * /dll_node/rate: 10.0
     * /dll_node/sensor_dev: 0.05
     * /dll_node/update_min_a: 0.01
     * /dll_node/update_min_d: 0.01
     * /dll_node/update_min_time: 0.1
     * /dll_node/use_imu: True
     * /rosdistro: noetic
     * /rosversion: 1.15.14
     * /use_sim_time: False

    NODES
      /
        dll_node (dll/dll_node)
        odom_tf (tf/static_transform_publisher)

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

    setting /run_id to d38f1aec-9e95-11ec-a0ea-cd8b567011ed
    process[rosout-1]: started with pid [22802]
    started core service [/rosout]
    process[odom_tf-2]: started with pid [22809]
    process[dll_node-3]: started with pid [22810]
    Octomap loaded
    Map size:
            x: -12.6 to 12.6
            y: -12.2 to 16.4
            z: -3 to 3.55
            Res: 0.05
    /home/kenmec/catkin_ws/src/LIO-SAM/test.grid not found!
    Computing 3D occupancy grid. This will take some time...
    Computing [dll_node-3] killing on exitCid map: 0.009064%        
    Computin[odom_tf-2] killing on exit  
    Computing grid map: 0.397514%    
    ```
    看到 **Computing grid map** 要等它跑完即可(Generating the .grid taaaaaaaaaaakes time, but it is only done once.)
    ```
    Computindone!d map: 100.000000%        
    Grid map successfully saved on /home/kenmec/catkin_ws/src/LIO-SAM/test.grid
    Computing trilinear interpolation map: 100% 
    ```
    載入地圖使用點雲格式需要億點時間等地圖出現至rviz
    

| method   | carto    | amcl3d   | hdl      | dll(dll) | dll(icp) |dll(ndt) |
| -------- | -------- | -------- | -------- | -------- |-------- |--------|
| map      | pbstream | -------- | pcd      |    bt |bt|bt|
| kidnap      |   v   |   --   |    | v | -- | -- |
| drift       | --  |   --  | 0.1m  | v |-- | --|
| problem     |      |  版本過舊(Kinetic)    |    | x軸持續震盪無法收斂 | icp(latency)無法即時定位 | 無法初始化位置(no input target dataset)|
| recommend   |  2    |   x   | 1   | 3  | x | x|

# 3D navigation

| method   | TEB    | follow   | 
| -------- | -------- | -------- | 
| behavior | 繞障 | 急停 | 
| env      |   一般路線   |   窄巷   | 





