modified lio_node.cpp

Author: yangjunXTU

.gitignore

@@ -1 +1,2 @@
-.vscode/
+.vscode/
+./Log/*

config/default.rviz

@@ -55,8 +55,8 @@ Visualization Manager:
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
-        Max Value: 3.1102588176727295
-        Min Value: -0.32467925548553467
+        Max Value: 3.1373021602630615
+        Min Value: -0.8680458664894104
         Value: true
       Axis: Z
       Channel Name: intensity
@@ -277,7 +277,7 @@ Visualization Manager:
       Axis: Z
       Channel Name: intensity
       Class: rviz/PointCloud2
-      Color: 255; 32; 99
+      Color: 15; 255; 143
       Color Transformer: FlatColor
       Decay Time: 0
       Enabled: true
@@ -290,8 +290,8 @@ Visualization Manager:
       Selectable: true
       Size (Pixels): 3
       Size (m): 0.03999999910593033
-      Style: Flat Squares
-      Topic: /mid360/ndt_clod
+      Style: Boxes
+      Topic: /mid360/undistort
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
@@ -324,25 +324,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 23.09314727783203
+      Distance: 31.872203826904297
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Field of View: 0.7853981852531433
       Focal Point:
-        X: 5.736478805541992
-        Y: -6.366806507110596
-        Z: -2.820770025253296
+        X: 8.03994369506836
+        Y: 6.020419120788574
+        Z: -2.2321622371673584
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.22479748725891113
+      Pitch: 0.7097973823547363
       Target Frame: <Fixed Frame>
-      Yaw: 0.9625622630119324
+      Yaw: 5.407115459442139
     Saved: ~
 Window Geometry:
   Displays:
@@ -363,4 +363,4 @@ Window Geometry:
     collapsed: false
   Width: 2457
   X: 280
-  Y: 297
+  Y: 223

src/lio_node.cpp

@@ -3,7 +3,7 @@
  * @Description:  
  * @Date: 2025-06-19 09:17:49
  * @LastEditors: yangjun_d 295967654@qq.com
- * @LastEditTime: 2025-08-20 12:27:33
+ * @LastEditTime: 2025-08-21 00:24:16
  */
 #include"lio_node.h"
 #include"utils/eigen_types.h"
@@ -401,13 +401,13 @@ void LIO::Undistort()
     scan_undistort_ = cloud;
     // TODO: 需要解决scan_undistort_、cloud数据类型不匹配的问题；
 
-    sensor_msgs::PointCloud2 output;
-    pcl::toROSMsg( *cloud, output );
-    output.header.frame_id = "map";
-    // output.header.stamp = ros::Time::fromSec(LidarMeasures.lidar_frame_end_time);
+    // sensor_msgs::PointCloud2 output;
+    // pcl::toROSMsg( *cloud, output );
+    // output.header.frame_id = "map";
+    // // output.header.stamp = ros::Time::fromSec(LidarMeasures.lidar_frame_end_time);
 
-    // std::cout<<"360 frame id : "<< output.header.frame_id << endl;
-    pub_pcl_un.publish( output );
+    // // std::cout<<"360 frame id : "<< output.header.frame_id << endl;
+    // pub_pcl_un.publish( output );
 
 }
 
@@ -452,9 +452,18 @@ void LIO::Align()
     CloudPtr current_scan_world(new PointCloudType);
     // pcl::PointCloud<pcl::PointXYZINormal>::Ptr current_scan_world{new pcl::PointXYZINormal};
     pcl::transformPointCloud(*current_scan_filter, *current_scan_world, current_pose.matrix());
+    pcl::transformPointCloud(*scan_undistort_, *scan_undistort_, current_pose.matrix());
+
+    sensor_msgs::PointCloud2 output1;
+    pcl::toROSMsg( *scan_undistort_, output1 );
+    output1.header.frame_id = "map";
+    // output.header.stamp = ros::Time::fromSec(LidarMeasures.lidar_frame_end_time);
+    
+    // std::cout<<"360 frame id : "<< output.header.frame_id << endl;
+    pub_pcl_un.publish( output1 );
 
     *pcl_wait_save += *current_scan_world;
-    pcl::concatenate(*pcl_wait_save, *current_scan_world, *pcl_wait_save);
+    // pcl::concatenate(*pcl_wait_save, *current_scan_world, *pcl_wait_save);
 
     if (delta_pose.translation().norm() > 1.0 || delta_pose.so3().log().norm() > deg2rad(10.0)) {
         // 将地图合入NDT中