add support to the Unitree LiDAR.

README.md

@@ -4,6 +4,8 @@
 **Personal repository** of our work SLAMesh, please raise issues **here** so that I can get reminders immediately.
 ### Update ###
 
+01/Jan/2025, add support for the Unitree L2 LiDAR.
+
 17/Aug/2023, Code released. Feel free to contact me with any questions. We are confident that this work introduced a novel approach to LiDAR SLAM, and we welcome everyone to explore opportunities in this approach. :two_men_holding_hands:
 
 10/Mar/2023, Preprint of our paper can be found on: [**paper**](https://arxiv.org/pdf/2303.05252.pdf 'title text'), [**slides**](https://drive.google.com/file/d/15xyX93LAsZ775Ywg8gNO_gJGThD4Hv3N/view?usp=sharing).
@@ -270,6 +272,18 @@ The number of LiDAR channels does not matter because our algorithm does not extr
 
 You can use our sample data recorded with an Ouster OS1-32 LiDAR: [SLAMesh dataset](https://connectpolyu-my.sharepoint.com/:f:/g/personal/21041552r_connect_polyu_hk/EjhEKl8-1GBLseA_2F7TOvEB3w7OyAJ_kS7DAaWoLay9ng?e=GK1fsd).
 
+For the Unitree LiDAR L2, their bags are available at [Unitree LiDAR L2 indoor](https://oss-global-cdn.unitree.com/static/L2%20Indoor%20Point%20Cloud%20Data.bag) and [Unitree LiDAR L2 outdoor](https://oss-global-cdn.unitree.com/static/L2%20Park%20Point%20Cloud%20Data.bag).
+```
+roslaunch slamesh slamesh_online_unitree.launch
+```
+<center>
+<img src="fig/unitree_l2_indoor.png" alt="unitree_l2_indoor" style="width:50%;" />
+</center>
+<center>
+<img src="fig/unitree_l2_outdoor.png" alt="unitree_l2_indoor" style="width:50%;" />
+</center>
+
+
 ### 3.4 About visualization
 
 Because mesh-tools rviz plugin do not support incremental mesh intersection, we provide three visualization modes controlled by `visualisation_type` in the param.yaml file:
@@ -285,7 +299,7 @@ lighter &#x2191;
 
 heavy &#x2193;
 
- after finish whole process, the global mesh map will be visualized in any mode
+ after finishing the whole process, the global mesh map will be visualized in any mode
 
 ## 4. Evaluation
 

config/online.yaml

@@ -10,13 +10,13 @@ slamesher:
   save_mesh_map: true
   #register param
   range_max: 60
-  range_min: 3.0
+  range_min: 0.5
   register_times: 5
   cross_overlap: true
   cross_cell_overlap_length: 1
-  num_margin_old_cell: 500
+  num_margin_old_cell: -1
   point2mesh: true
-  residual_combination: true
+  residual_combination: false
   #visualize parameter
   meshing_tsdf: false
   full_cover: true

config/online_unitree.yaml

@@ -0,0 +1,45 @@
+slamesher:
+  # play ros bag or run it in mobile robot, the configuration used in our real-world demo, the sensor is an ouster OS1-32 lidar
+  #read param
+  use_sim_time: true
+  grt_available: false 
+  odom_available: false 
+  read_offline_pcd: false
+  dataset: 1
+  max_steps: 10000
+  save_mesh_map: true
+  #register param
+  range_max: 40
+  range_min: 0.0
+  register_times: 6
+  cross_overlap: false
+  cross_cell_overlap_length: 1
+  num_margin_old_cell: -1
+  point2mesh: true
+  residual_combination: false
+  #visualize parameter
+  meshing_tsdf: false
+  full_cover: true
+  visualisation_type: 1
+  #gp param
+  num_thread: 8
+  grid: 1.2
+  min_points_num_to_gp: 3
+  num_test: 6
+
+  variance_register: 0.60
+  variance_map_update: 0.50
+  variance_map_show: 0.20
+  variance_min: 1.36
+  variance_sensor: 0.02
+
+  correction_x: 0
+  correction_y: 0
+  correction_z: 0
+  correction_roll_degree: 0
+  correction_pitch_degree: -90
+  correction_yaw_degree: 0
+
+  test_param:  0
+
+

config/rviz/slamesh_mesh_tool.rviz

@@ -6,12 +6,11 @@ Panels:
       Expanded:
         - /Global Options1
         - /Grid1/Line Style1
-        - /registered_points1
         - /odom1
         - /normal1/Namespaces1
         - /updated_mesh1/Display Type1
       Splitter Ratio: 0.5621469020843506
-    Tree Height: 138
+    Tree Height: 1568
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -39,9 +38,9 @@ Visualization Manager:
   Class: ""
   Displays:
     - Alpha: 0.5
-      Cell Size: 10
+      Cell Size: 1
       Class: rviz/Grid
-      Color: 96; 96; 99
+      Color: 238; 238; 236
       Enabled: false
       Line Style:
         Line Width: 0.029999999329447746
@@ -53,16 +52,23 @@ Visualization Manager:
         Y: 0
         Z: 0
       Plane: XY
-      Plane Cell Count: 150
+      Plane Cell Count: 10
       Reference Frame: <Fixed Frame>
       Value: false
     - Class: rviz/Axes
       Enabled: true
-      Length: 5
+      Length: 1
       Name: Axes
-      Radius: 0.20000000298023224
+      Radius: 0.10000000149011612
       Reference Frame: <Fixed Frame>
       Value: true
+    - Class: rviz/Axes
+      Enabled: true
+      Length: 1
+      Name: Axes
+      Radius: 0.10000000149011612
+      Reference Frame: slamesher_odom
+      Value: true
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
@@ -83,19 +89,19 @@ Visualization Manager:
       Position Transformer: XYZ
       Queue Size: 10
       Selectable: true
-      Size (Pixels): 1
+      Size (Pixels): 2
       Size (m): 0.009999999776482582
       Style: Points
       Topic: /os_cloud_node/points
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
       Value: true
-    - Alpha: 0.30000001192092896
+    - Alpha: 0.10000000149011612
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
-        Max Value: 5.296406269073486
-        Min Value: -10.943437576293945
+        Max Value: 3.902271032333374
+        Min Value: -0.19966024160385132
         Value: true
       Axis: Z
       Channel Name: intensity
@@ -111,7 +117,7 @@ Visualization Manager:
       Position Transformer: XYZ
       Queue Size: 10
       Selectable: true
-      Size (Pixels): 1
+      Size (Pixels): 3
       Size (m): 0.009999999776482582
       Style: Points
       Topic: /raw_points_in_world
@@ -124,16 +130,16 @@ Visualization Manager:
         - Alpha: 1
           Autocompute Intensity Bounds: true
           Autocompute Value Bounds:
-            Max Value: 6.266666889190674
-            Min Value: -11.066666603088379
+            Max Value: 85.26667022705078
+            Min Value: -17.568307876586914
             Value: true
           Axis: Z
           Channel Name: intensity
           Class: rviz/PointCloud
           Color: 136; 138; 133
           Color Transformer: AxisColor
           Decay Time: 0
-          Enabled: true
+          Enabled: false
           Invert Rainbow: false
           Max Color: 255; 255; 255
           Min Color: 0; 0; 0
@@ -148,7 +154,7 @@ Visualization Manager:
           Unreliable: false
           Use Fixed Frame: true
           Use rainbow: true
-          Value: true
+          Value: false
         - Alpha: 1
           Autocompute Intensity Bounds: true
           Autocompute Value Bounds:
@@ -217,7 +223,7 @@ Visualization Manager:
           Color: 255; 255; 255
           Color Transformer: FlatColor
           Decay Time: 0
-          Enabled: true
+          Enabled: false
           Invert Rainbow: false
           Max Color: 255; 255; 255
           Min Color: 0; 0; 0
@@ -232,7 +238,7 @@ Visualization Manager:
           Unreliable: false
           Use Fixed Frame: true
           Use rainbow: true
-          Value: true
+          Value: false
         - Alpha: 1
           Autocompute Intensity Bounds: true
           Autocompute Value Bounds:
@@ -270,7 +276,7 @@ Visualization Manager:
           Axis: Z
           Channel Name: intensity
           Class: rviz/PointCloud
-          Color: 204; 0; 0
+          Color: 237; 212; 0
           Color Transformer: FlatColor
           Decay Time: 0
           Enabled: true
@@ -282,7 +288,7 @@ Visualization Manager:
           Queue Size: 10
           Selectable: true
           Size (Pixels): 2
-          Size (m): 0.20000000298023224
+          Size (m): 0.05000000074505806
           Style: Spheres
           Topic: /pose_pub
           Unreliable: false
@@ -412,7 +418,7 @@ Visualization Manager:
           Value: false
           Vertex Costs Lower Limit: 0
           Vertex Costs Upper Limit: 1
-        Value: Vertex Color
+        Value: Hide Faces
         Vertex Color Service Name: get_vertex_colors
         Vertex Colors Topic: ""
         Vertex Costs Topic: ""
@@ -432,7 +438,7 @@ Visualization Manager:
       Value: true
   Enabled: true
   Global Options:
-    Background Color: 219; 222; 231
+    Background Color: 211; 215; 207
     Default Light: true
     Fixed Frame: map
     Frame Rate: 30
@@ -458,33 +464,33 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 198.70970153808594
+      Distance: 35.379547119140625
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -1.0801451206207275
-        Y: 72.43070983886719
-        Z: -75.27633666992188
+        X: -8.385961532592773
+        Y: 3.0867207050323486
+        Z: -4.36432409286499
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 1.0097962617874146
+      Pitch: 1.0597960948944092
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 5.113673210144043
+      Yaw: 1.5968458652496338
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: true
   Height: 2033
   Hide Left Dock: true
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd0000000400000000000002c60000079cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed0000043c00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000270000079c0000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f00000755fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000006e000007550000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006500000000000000073f000008d800fffffffb0000000800540069006d0065010000000000000450000000000000000000000e840000079c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000002c60000079cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed0000043c00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000270000079c0000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f00000755fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000006e000007550000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006500000000000000073f000008d800fffffffb0000000800540069006d0065010000000000000450000000000000000000000e640000079c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -493,6 +499,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: true
-  Width: 3716
-  X: 124
-  Y: 1139
+  Width: 3684
+  X: 156
+  Y: 55

launch/slamesh_online.launch

@@ -8,7 +8,7 @@
     <node pkg="slamesh" type="slamesh" name="slamesher" output="screen">
         <!--
          <remap from="/velodyne_points" to="/points_raw" />
-         <remap from="/velodyne_points" to="/os_cloud_node/points" />
+         <remap from="/velodyne_points" to="/os_cloud_node/points" "/unilidar/cloud" />
 -->
         <remap from="/velodyne_points" to="/os_cloud_node/points" />
         <param name="seq" value="$(arg seq)"/>

launch/slamesh_online_unitree.launch

@@ -0,0 +1,21 @@
+<launch>
+    <arg name="seq" default = "/00" />
+    <arg name="param" default = "" /> <!-- param is used for tuning "$(arg param)" -->
+    <arg name="data_path" default = " " />
+    <arg name="result_path" default = "$(find slamesh)/../../slamesh_result/kitti" />
+
+    <rosparam file="$(find slamesh)/config/online_unitree.yaml" command="load" />
+    <node pkg="slamesh" type="slamesh" name="slamesher" output="screen">
+        <!--
+         <remap from="/velodyne_points" to="/points_raw" />
+         <remap from="/velodyne_points" to="/os_cloud_node/points" "/unilidar/cloud" />
+-->
+        <remap from="/velodyne_points" to="/unilidar/cloud"  />
+        <param name="seq" value="$(arg seq)"/>
+        <param name="file_loc_report"  value="$(arg result_path)"/>
+        <param name="file_loc_dataset" value="$(arg data_path)"/>
+    </node>
+
+    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find slamesh)/config/rviz/slamesh_mesh_tool.rviz" required="false" />
+</launch>
+

src/map.cpp

@@ -36,6 +36,7 @@ bool getPointCloud(PointMatrix & points_result, pcl::PointCloud<pcl::PointXYZ> &
             first = false;
         }
         if(!g_data.pcl_msg_buff_deque.empty()){
+            //get the message
             pcl::fromROSMsg(g_data.pcl_msg_buff_deque.front(), *pcl_raw_ptr);
             get_laser_time = g_data.pcl_msg_buff_deque.front().header.stamp.toSec();
             g_data.pcl_msg_buff = g_data.pcl_msg_buff_deque.front();