3D mapping - HackMD

# 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 | 一般路線 | 窄巷 |

Syntax	Example	Reference
# Header	Header	基本排版
- Unordered List	Unordered List
1. Ordered List	Ordered List
- [ ] Todo List	Todo List
> Blockquote	Blockquote
Bold font	Bold font
Italics font	Italics font
~~Strikethrough~~	~~Strikethrough~~
19^th^	19^th
H~2~O	H₂O
++Inserted text++	Inserted text
==Marked text==	Marked text
[link text](https:// "title")	Link
![image alt](https:// "title")	Image
`Code`	`Code`	在筆記中貼入程式碼
```javascript var i = 0; ```	`var i = 0;`
:smile:		Emoji list
{%youtube youtube_id %}	Externals
$L^aT_eX$	L^aT_eX
:::info This is a alert area. :::	This is a alert area.