solve some bugs and increase the precission

hku-mars · Nov 12, 2020 · 47e9f27 · 47e9f27
1 parent 1cc9cfe
commit 47e9f27
Show file tree

Hide file tree

Showing 5 changed files with 358 additions and 447 deletions.
diff --git a/launch/mapping_avia.launch b/launch/mapping_avia.launch
@@ -20,18 +20,18 @@
 	<param name="edgeb" type="double" value="0.1"/>
 	<param name="smallp_intersect" type="double" value="172.5"/>
 	<param name="smallp_ratio" type="double" value="1.2"/>
-	<param name="point_filter_num" type="int" value="5"/>
+	<param name="point_filter_num" type="int" value="4"/>
   <node pkg="fast_lio" type="loam_feat_extract" name="feature_extract" output="screen"/>
 
   <node pkg="fast_lio" type="loam_laserMapping" name="laserMapping" output="screen">
   <param name="map_file_path" type="string" value=" " />
   <param name="max_iteration" type="int" value="2" />
   <param name="dense_map_enable" type="bool" value="true" />
-  <param name="fov_degree" type="double" value="80" />
+  <param name="fov_degree" type="double" value="75" />
   <param name="filter_size_corner" type="double" value="0.3" />
   <param name="filter_size_surf" type="double" value="0.2" />
   <param name="filter_size_map" type="double" value="0.2" />
-  <param name="cube_side_length" type="double" value="15" />
+  <param name="cube_side_length" type="double" value="10" />
   </node>
 
   <group if="$(arg rviz)">

diff --git a/rviz_cfg/loam_livox.rviz b/rviz_cfg/loam_livox.rviz
@@ -217,8 +217,8 @@ Visualization Manager:
         - Alpha: 0.20000000298023224
           Autocompute Intensity Bounds: true
           Autocompute Value Bounds:
-            Max Value: -1.7674484252929688
-            Min Value: -8.243647575378418
+            Max Value: 0.32742905616760254
+            Min Value: -2.179872751235962
             Value: true
           Axis: Z
           Channel Name: intensity
@@ -664,18 +664,18 @@ Visualization Manager:
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 6.608828544616699
-        Y: 3.081573247909546
-        Z: -3.704350709915161
+        X: 6.49392557144165
+        Y: -0.4694555699825287
+        Z: -1.3836920261383057
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: -0.17020323872566223
+      Pitch: 0.3497962951660156
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 3.0474979877471924
+      Yaw: 5.425689220428467
     Saved: ~
 Window Geometry:
   Displays:

diff --git a/src/IMU_Processing.hpp b/src/IMU_Processing.hpp
@@ -26,7 +26,7 @@
 
 /// *************Preconfiguration
 
-#define MAX_INI_COUNT (50)
+#define MAX_INI_COUNT (200)
 
 const bool time_list(PointType &x, PointType &y) {return (x.curvature < y.curvature);};
 
@@ -216,15 +216,15 @@ void ImuProcess::UndistortPcl(const MeasureGroup &meas, StatesGroup &state_inout
 
     #ifdef DEBUG_PRINT
     // fout<<head->header.stamp.toSec()<<" "<<angvel_avr.transpose()<<" "<<acc_avr.transpose()<<std::endl;
-    #endif
+    #endif  
     dt = tail->header.stamp.toSec() - head->header.stamp.toSec();
 
     /* covariance propagation */
     Eigen::Matrix3d acc_avr_skew;
     Eigen::Matrix3d Exp_f   = Exp(angvel_avr, dt);
     acc_avr_skew<<SKEW_SYM_MATRX(angvel_avr);
 
-    F_x.block<3,3>(0,0)  = - Exp_f;
+    F_x.block<3,3>(0,0)  = Exp(angvel_avr, - dt);
     F_x.block<3,3>(0,9)  = - Eye3d * dt;
     // F_x.block<3,3>(3,0)  = R_imu * off_vel_skew * dt;
     F_x.block<3,3>(3,6)  = Eye3d * dt;
@@ -290,7 +290,6 @@ void ImuProcess::UndistortPcl(const MeasureGroup &meas, StatesGroup &state_inout
     pos_imu<<VEC_FROM_ARRAY(head->pos);
     angvel_avr<<VEC_FROM_ARRAY(head->gyr);
 
-    int i = 0;
     for(; it_pcl->curvature / double(1000) > head->offset_time; it_pcl --)
     {
       dt = it_pcl->curvature / double(1000) - head->offset_time;