add pcl_viewer tips

README.md

@@ -5,7 +5,7 @@
 3. [UAV Avoiding Dynamic Obstacles](https://github.com/hku-mars/dyn_small_obs_avoidance): One of the implementation of FAST-LIO in robot's planning.
 4. [R2LIVE](https://github.com/hku-mars/r2live): A high-precision LiDAR-inertial-Vision fusion work using FAST-LIO as LiDAR-inertial front-end.
 5. [UGV Demo](https://www.youtube.com/watch?v=wikgrQbE6Cs): Model Predictive Control for Trajectory Tracking on Differentiable Manifolds.
-6. [SC-A-LOAM](https://github.com/gisbi-kim/SC-A-LOAM#for-livox-lidar): A scan-context loop closure module that can directly work with FAST-LIO1 (The support for FAST-LIO2 is under developing).
+6. [SC-A-LOAM](https://github.com/gisbi-kim/SC-A-LOAM#for-livox-lidar): A [Scan-Context](https://github.com/irapkaist/scancontext) loop closure module that can directly work with FAST-LIO1 (The support for FAST-LIO2 is under developing).
 
 ## FAST-LIO
 **FAST-LIO** (Fast LiDAR-Inertial Odometry) is a computationally efficient and robust LiDAR-inertial odometry package. It fuses LiDAR feature points with IMU data using a tightly-coupled iterated extended Kalman filter to allow robust navigation in fast-motion, noisy or cluttered environments where degeneration occurs. Our package address many key issues:
@@ -139,7 +139,17 @@ Step C: Run LiDAR's ros driver or play rosbag.
 
 ### 3.4 PCD file save
 
-Set ``` pcd_save_enable ``` in launchfile to ``` 1 ```. All the scans (in global frame) will be accumulated and saved to the file ``` FAST_LIO/PCD/scans.pcd ``` after the FAST-LIO is terminated.
+Set ``` pcd_save_enable ``` in launchfile to ``` 1 ```. All the scans (in global frame) will be accumulated and saved to the file ``` FAST_LIO/PCD/scans.pcd ``` after the FAST-LIO is terminated. ```pcl_viewer scans.pcd``` can visualize the point clouds.
+
+*Tips for pcl_viewer:*
+- change what to visualize/color by pressing keyboard 1,2,3,4,5 when pcl_viewer is running. 
+```
+    1 is all random
+    2 is X values
+    3 is Y values
+    4 is Z values
+    5 is intensity
+```
 
 ## 4. Rosbag Example
 ### 4.1 Indoor rosbag (Livox Avia LiDAR)

src/laserMapping.cpp

@@ -289,6 +289,7 @@ void livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg)
     mtx_buffer.lock();
     scan_count ++;
     double preprocess_start_time = omp_get_wtime();
+    cout<<"lidar got at: "<<msg->header.stamp.toSec()<<endl;
     if (msg->header.stamp.toSec() < last_timestamp_lidar)
     {
         ROS_ERROR("lidar loop back, clear buffer");
@@ -307,6 +308,7 @@ void livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg)
 void imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in) 
 {
     publish_count ++;
+    cout<<"IMU got at: "<<msg_in->header.stamp.toSec()<<endl;
     sensor_msgs::Imu::Ptr msg(new sensor_msgs::Imu(*msg_in));
 
     double timestamp = msg->header.stamp.toSec();
@@ -347,6 +349,11 @@ bool sync_packages(MeasureGroup &meas)
         lidar_pushed = true;
     }
 
+    if (abs(last_timestamp_imu - lidar_end_time) > 10.0)
+    {
+        printf("IMU and LiDAR not Synced, IMU time: %lf, lidar scan end time: %lf",last_timestamp_imu, lidar_end_time);
+    }
+
     if (last_timestamp_imu < lidar_end_time)
     {
         return false;
@@ -432,6 +439,7 @@ void publish_frame_world(const ros::Publisher & pubLaserCloudFullRes)
     {
         RGBpointBodyToWorld(&laserCloudFullRes->points[i], \
                             &laserCloudWorld->points[i]);
+        cout<<laserCloudWorld->points[i].intensity<<endl;
     }
 
     // *pcl_wait_pub += *laserCloudWorld;