From fa49407d1cc09af74a250a456ce5ac039893a249 Mon Sep 17 00:00:00 2001
From: xw <buaaxw@gmail.com>
Date: Tue, 6 Jul 2021 01:41:46 -0400
Subject: [PATCH] fix print

---
 config/avia.yaml         |  2 +-
 config/horizon.yaml      |  2 +-
 rviz_cfg/loam_livox.rviz | 18 +++++++++---------
 src/IMU_Processing.hpp   | 16 ++++++----------
 src/laserMapping.cpp     |  2 +-
 5 files changed, 18 insertions(+), 22 deletions(-)

diff --git a/config/avia.yaml b/config/avia.yaml
index 5b3e57cd..921ececf 100644
--- a/config/avia.yaml
+++ b/config/avia.yaml
@@ -5,7 +5,7 @@ common:
 preprocess:
     lidar_type: 1                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 
     scan_line: 6
-    blind: 1
+    blind: 4
 
 mapping:
     acc_cov: 0.1
diff --git a/config/horizon.yaml b/config/horizon.yaml
index 293138f9..b4774ee7 100644
--- a/config/horizon.yaml
+++ b/config/horizon.yaml
@@ -5,7 +5,7 @@ common:
 preprocess:
     lidar_type: 1                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 
     scan_line:  6
-    blind: 1
+    blind: 4
 
 mapping:
     acc_cov: 0.1
diff --git a/rviz_cfg/loam_livox.rviz b/rviz_cfg/loam_livox.rviz
index 553f1070..768e6764 100644
--- a/rviz_cfg/loam_livox.rviz
+++ b/rviz_cfg/loam_livox.rviz
@@ -86,7 +86,7 @@ Visualization Manager:
           Enabled: true
           Invert Rainbow: false
           Max Color: 255; 255; 255
-          Max Intensity: 202
+          Max Intensity: 255
           Min Color: 0; 0; 0
           Min Intensity: 0
           Name: surround
@@ -212,7 +212,7 @@ Visualization Manager:
       Buffer Length: 2
       Class: rviz/Path
       Color: 25; 255; 255
-      Enabled: true
+      Enabled: false
       Head Diameter: 0
       Head Length: 0
       Length: 0.30000001192092896
@@ -230,7 +230,7 @@ Visualization Manager:
       Shaft Length: 0.4000000059604645
       Topic: /path
       Unreliable: false
-      Value: true
+      Value: false
     - Alpha: 1
       Autocompute Intensity Bounds: false
       Autocompute Value Bounds:
@@ -327,25 +327,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/ThirdPersonFollower
-      Distance: 207.86837768554688
+      Distance: 122.74785614013672
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -66.09013366699219
-        Y: 46.88283920288086
-        Z: 3.041166019102093e-5
+        X: 17.150386810302734
+        Y: 1.2471342086791992
+        Z: 3.980784458690323e-5
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.8397973775863647
+      Pitch: 1.189797043800354
       Target Frame: camera_init
       Value: ThirdPersonFollower (rviz)
-      Yaw: 1.4935526847839355
+      Yaw: 3.293565511703491
     Saved: ~
 Window Geometry:
   Displays:
diff --git a/src/IMU_Processing.hpp b/src/IMU_Processing.hpp
index cd383bcc..76958394 100644
--- a/src/IMU_Processing.hpp
+++ b/src/IMU_Processing.hpp
@@ -103,7 +103,7 @@ ImuProcess::~ImuProcess() {}
 
 void ImuProcess::Reset() 
 {
-  ROS_WARN("Reset ImuProcess");
+  // ROS_WARN("Reset ImuProcess");
   mean_acc      = V3D(0, 0, -1.0);
   mean_gyr      = V3D(0, 0, 0);
   angvel_last       = Zero3d;
@@ -158,7 +158,7 @@ void ImuProcess::IMU_init(const MeasureGroup &meas, esekfom::esekf<state_ikfom,
 {
   /** 1. initializing the gravity, gyro bias, acc and gyro covariance
    ** 2. normalize the acceleration measurenments to unit gravity **/
-  ROS_INFO("IMU Initializing: %.1f %%", double(N) / MAX_INI_COUNT * 100);
+  
   V3D cur_acc, cur_gyr;
   
   if (b_first_frame_)
@@ -171,7 +171,6 @@ void ImuProcess::IMU_init(const MeasureGroup &meas, esekfom::esekf<state_ikfom,
     mean_acc << imu_acc.x, imu_acc.y, imu_acc.z;
     mean_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z;
     first_lidar_time = meas.lidar_beg_time;
-    // cout<<"init acc norm: "<<mean_acc.norm()<<endl;
   }
 
   for (const auto &imu : meas.imu)
@@ -209,6 +208,7 @@ void ImuProcess::IMU_init(const MeasureGroup &meas, esekfom::esekf<state_ikfom,
   init_P(21,21) = init_P(22,22) = 0.00001; 
   kf_state.change_P(init_P);
   last_imu_ = meas.imu.back();
+
 }
 
 void ImuProcess::UndistortPcl(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, PointCloudXYZI &pcl_out)
@@ -355,16 +355,12 @@ void ImuProcess::Process(const MeasureGroup &meas,  esekfom::esekf<state_ikfom,
     {
       cov_acc *= pow(G_m_s2 / mean_acc.norm(), 2);
       imu_need_init_ = false;
-      // ROS_INFO("IMU Initials: Gravity: %.4f %.4f %.4f %.4f; state.bias_g: %.4f %.4f %.4f; acc covarience: %.8f %.8f %.8f; gry covarience: %.8f %.8f %.8f",\
-      //          imu_state.grav[0], imu_state.grav[1], imu_state.grav[2], mean_acc.norm(), cov_acc_scale[0], cov_acc_scale[1], cov_acc_scale[2], cov_acc[0], cov_acc[1], cov_acc[2], cov_gyr[0], cov_gyr[1], cov_gyr[2]);
-      // cov_acc = cov_acc.cwiseProduct(cov_acc_scale);
-      // cov_gyr = cov_gyr.cwiseProduct(cov_gyr_scale);
 
       cov_acc = cov_acc_scale;
       cov_gyr = cov_gyr_scale;
-      // cout<<"mean acc: "<<mean_acc<<" acc measures in word frame:"<<state.rot_end.transpose()*mean_acc<<endl;
-      ROS_INFO("IMU Initials: Gravity: %.4f %.4f %.4f %.4f; state.bias_g: %.4f %.4f %.4f; acc covarience: %.8f %.8f %.8f; gry covarience: %.8f %.8f %.8f",\
-               imu_state.grav[0], imu_state.grav[1], imu_state.grav[2], mean_acc.norm(), cov_bias_gyr[0], cov_bias_gyr[1], cov_bias_gyr[2], cov_acc[0], cov_acc[1], cov_acc[2], cov_gyr[0], cov_gyr[1], cov_gyr[2]);
+      ROS_INFO("IMU Initial Done");
+      // ROS_INFO("IMU Initial Done: Gravity: %.4f %.4f %.4f %.4f; state.bias_g: %.4f %.4f %.4f; acc covarience: %.8f %.8f %.8f; gry covarience: %.8f %.8f %.8f",\
+      //          imu_state.grav[0], imu_state.grav[1], imu_state.grav[2], mean_acc.norm(), cov_bias_gyr[0], cov_bias_gyr[1], cov_bias_gyr[2], cov_acc[0], cov_acc[1], cov_acc[2], cov_gyr[0], cov_gyr[1], cov_gyr[2]);
       fout_imu.open(DEBUG_FILE_DIR("imu.txt"),ios::out);
     }
 
diff --git a/src/laserMapping.cpp b/src/laserMapping.cpp
index d275ea14..5a5fa1af 100644
--- a/src/laserMapping.cpp
+++ b/src/laserMapping.cpp
@@ -763,7 +763,7 @@ int main(int argc, char** argv)
             {
                 first_lidar_time = Measures.lidar_beg_time;
                 p_imu->first_lidar_time = first_lidar_time;
-                cout<<"FAST-LIO not ready"<<endl;
+                // cout<<"FAST-LIO not ready"<<endl;
                 continue;
             }